Adaptive Sliding Mode Tracking Control Method for Posture and Trajectory Integration of Spiral Vascular Robot

Abstract

A method for integrated posture and trajectory adaptive sliding mode tracking control of a spiral vascular robot belongs to the field of control technology. The purpose of the present invention is to fully consider the posture and trajectory coupling of the vascular robot and the influence of gravity - buoyancy, and improve the control accuracy of the integrated posture and trajectory adaptive sliding mode tracking control method of the spiral vascular robot. The steps of the present invention are: establishing an integrated kinematic and dynamic model of the spiral vascular robot; designing a gravity - buoyancy compensator to compensate for the sinking effect of gravity - buoyancy in the vertical direction; designing a sliding mode controller based on gravity - buoyancy compensation; and designing an adaptive sliding mode control based on the sliding mode controller designed in step three. The present invention can better suppress the jitter phenomenon of sliding mode control and improve the accuracy of attitude trajectory tracking control. It has great practical value and theoretical significance for the application of vascular robots in minimally invasive surgery.

Description

Technical Field

[0001] The present invention belongs to the field of control technology. Background Art

[0002] In recent years, while science and technology have been booming, irregular living habits have troubled an increasing number of people with cardiovascular and cerebrovascular diseases. Due to the disadvantages of traditional surgical treatments, such as large radiation damage, large postoperative wounds, and slow recovery, the unique characteristics of minimally invasive surgery enable it to overcome the deficiencies of traditional surgical treatments, thus becoming a hot topic in the medical field. Combining minimally invasive surgery with robotic technology has become the mainstream development trend in the medical field and has broad development prospects.

[0003] A vascular robot is a new method for addressing cardiovascular and cerebrovascular diseases. A vascular robot refers to a micro-robot that operates in a vascular environment and can meet the working requirements through an external controller. In daily life, a vascular robot can not only be used for targeted treatment of cancerous tissues and precisely deliver drugs to the affected areas; it can also be used to regularly remove blood deposits such as cholesterol and fat to prevent cardiovascular and cerebrovascular diseases. At the same time, a vascular robot also has corresponding detection functions and can diagnose and detect various organs and tissues of the human body. Doctors can adopt reasonable treatment methods based on the detection data of the vascular robot, ultimately greatly reducing surgical operations and minimizing surgical harm to patients. In addition, a vascular robot also has functions such as removing parasites and bacteria, cleaning wounds, and crushing stones, which can not only effectively solve patients' disease problems but also facilitate people's lives. With the assistance of a vascular robot, the harm caused by traditional surgeries can be significantly reduced, so the research on vascular robots has gradually become an important research direction in the medical field.

[0004] The vascular robot with a spiral structure consists of a magnetic material head and a spiral tail, and is powered by an external three-dimensional rotating magnetic field. The hydrodynamic lubrication effect generated by the spiral motion of the robot can effectively avoid the contact between the robot and the blood vessel wall and protect the blood vessels. After comprehensively considering the safety and the stability and feasibility of the energy supply method, the vascular robot with a spiral structure driven by an external rotating magnetic field has high academic research value and broad medical application prospects. Therefore, the object of the present invention is the spiral vascular robot. Traditionally, researchers have modeled the motion trajectory and attitude of the vascular robot separately, but the actual motion of the vascular robot is affected by the coupling of the attitude and trajectory. According to the working requirements of the vascular robot, it is necessary to ensure that the vascular robot does not damage the human blood vessel wall. However, in the actual working process, due to the influence of gravity-buoyancy in the vertical direction, the vascular robot often sinks and deviates relative to the expected motion trajectory. Moreover, the traditional control method has poor control effect on the vascular robot. To avoid the above problems, when modeling and controlling the vascular robot, the above actual situation should be taken into account. Therefore, finding a modeling solution that can fully consider the influence of the attitude-trajectory coupling of the vascular robot and a control solution that can consider gravity-buoyancy compensation is of great significance for the application of the vascular robot in minimally invasive surgery. Summary of the Invention

[0005] The object of the present invention is a method for attitude-trajectory integrated adaptive sliding mode tracking control of a spiral vascular robot that fully considers the influence of the attitude-trajectory coupling and gravity-buoyancy of the vascular robot and improves the control accuracy.

[0006] The steps of the present invention are as follows:

[0007] Step 1: Use dual quaternions to simultaneously describe the attitude and trajectory motion of the spiral vascular robot, and establish the attitude-trajectory integrated kinematic and dynamic models of the spiral vascular robot;

[0008] Step 2: When the spiral vascular robot moves in the blood, it is affected by gravity-buoyancy, resulting in the actual motion trajectory sinking and deviating, and deviating from the expected trajectory. Design a gravity-buoyancy compensator to compensate for the sinking effect of gravity-buoyancy in the vertical direction;

[0009] Step 3: Based on the spiral vascular robot model established in Step 1 and the gravity-buoyancy compensator designed in Step 2, design a sliding mode controller based on gravity-buoyancy compensation;

[0010] Step 4: Based on the sliding mode controller designed in Step 3, design an adaptive sliding mode control;

[0011] The specific process of establishing the attitude-trajectory integrated kinematic and dynamic models described in Step 1 is as follows:

[0012] The body coordinate system O of the vascular robot described by unit dual quaternion b X b Y b Z b With respect to the desired coordinate system O d X d Y d Z d The pose and trajectory error is:

[0013]

[0014] where and are the pose and trajectory of the desired coordinate system O of the vascular robot represented by unit dual quaternion d X d Y d Z d and the body coordinate system O b X b Y b Z b with respect to the inertial coordinate system OXYZ respectively. "*" represents the multiplication of dual quaternions, represents the conjugate of the dual quaternion;

[0015] The body coordinate system O of the vascular robot b X b Y b Z b With respect to the desired coordinate system O d X d Y d Z d The velocity screw error in the coordinate system O b X b Y b Z b is expressed as:

[0016]

[0017] where is the representation of the velocity screw of the body coordinate system O of the vascular robot b X b Y b Z b with respect to the inertial coordinate system OXYZ in the body coordinate system O of the robot b X b Y b Z b and is the desired coordinate system O of the vascular robot d X d Y d Zd The velocity screw with respect to the inertial coordinate system OXYZ is represented in the desired coordinate system O d X d Y d Z d by (·) * where (·)

[0018] Furthermore, through derivation, the integrated kinematic and dynamic equations of attitude and orbit based on can be obtained as follows:

[0019]

[0020]

[0021] where M b is the dual mass property inertia matrix, represents the dual force screw with the acting point at the centroid of the vascular robot in the coordinate system O b X b Y b Z b and "×" represents the cross product operation of dual quaternions. represents an operation between a matrix and a dual quaternion. For the dual quaternion it is defined that the operation of where ε represents the dual unit; it is defined that the operation of is any dual quaternion;

[0022] The specific design process of the gravity-buoyancy compensator described in Step 2 is as follows:

[0023] When the vascular robot moves in the blood, the desired motion speed of the vascular robot is defined as The vascular robot in the blood is mainly subjected to the total driving force the gravity-buoyancy of the robot and the fluid resistance .

[0024] The expression of the total driving force is:

[0025]

[0026] where represents the driving force generated by the rotation of the spiral tail of the vascular robot under the action of a three-dimensional rotating magnetic field, is the magnetic pulling force generated by the magnetic gradient, n is the number of turns of the spiral tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, and α 1is the helix lift angle of the tail of the vascular robot, is the rotational angular velocity of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity, is the magnetic field gradient, ζ ⊥ is the axial resistance coefficient perpendicular to the helical tail, and ζ 11 is the axial resistance coefficient parallel to the helical tail, and η is the blood viscosity coefficient, κ is the diameter of the helical tail of the vascular robot;

[0027] Fluid resistance is the fluid resistance generated by the impact of the blood flow on the head and helical tail of the vascular robot and is expressed as:

[0028]

[0029] Robot gravity - buoyancy is the resultant force of gravity G and buoyancy acting in the vertical direction, and its expression is:

[0030]

[0031] where ρ is the density of the vascular robot, ρ f is the density of the blood, is the acceleration due to gravity;

[0032] To offset the effect of gravity - buoyancy on the vascular robot, fluid resistance and the total driving force are required to act together, that is:

[0033]

[0034] where is to design a gravity - buoyancy compensator and establish the attitude coordinate system p of the vascular robot. The origin O of the coordinate system p is located at the center of gravity of the vascular robot, and the x - axis of the coordinate system p coincides with the actual direction of the vascular robot and the z - axis p is perpendicular to the actual direction of the vascular robot is the moving speed of the vascular robot to offset the influence of gravity - buoyancy,

[0035] According to Equation (8), we have:

[0036]

[0037] Decompose the expected moving speed of the vascular robot into a horizontal component and a vertical component It can be known that the expected moving speed of the vascular robot horizontal component and horizontal component are equal, that is, there is The angular velocity of rotation of the vascular robot and the angular velocity of rotation Ω of the three-dimensional rotating magnetic field 1 have the following relationship:

[0038]

[0039] According to the above analysis, it can be obtained that:

[0040]

[0041] wherein, is a unit vector;

[0042] When , according to equations (9) and (10), it can be obtained that:

[0043]

[0044] When , decompose into two forces, that is, the component parallel to the x p axis and the component perpendicular to the x p axis f ⊥p has no component in the direction. Also, due to the uncertainty and variability of the magnetic field gradient direction, assume that acts on the x p z p plane and the direction is axisymmetric with respect to the x axis and the expected moving speed of the vascular robot p . Then, similar to the force analysis of , can be decomposed into the component parallel to the x p axis and the component perpendicular to the x p axis f ⊥p_1 has no component in the direction, then there is:

[0045]

[0046] wherein, and respectively represent the numerical values of the projections on the z p axis and the x p axis, and At z p axis and the numerical value projected onto the x p axis, respectively representing the unit vectors in the z axis direction and the x p axis direction; substituting Eqs. (10) and (13) into Eq. (9), we get: p Substituting Eqs. (10) and (13) into Eq. (9), we obtain:

[0047]

[0048] Since the desired pitch angle of the vascular robot is defined as Project the velocity onto the x p axis and the z p axis directions, and according to Eq. (14), we have:

[0049]

[0050] By analyzing the mechanical and geometric relationships of the attitude coordinate system p of the vascular robot, we obtain:

[0051]

[0052] where β is the angle by which the desired pitch angle of the vascular robot tilts upward;

[0053] Similar to the analysis of in Eq. (16), we can also obtain the following relationship:

[0054]

[0055] According to Eqs. (15), (16), and (17), we get:

[0056]

[0057] According to cos(θ + β) = cos(θ)cos(β) - sin(θ)sin(β), from Eq. (18), the angle β by which the vascular robot output by the gravity-buoyancy compensator tilts upward compared to the ideal pitch angle is:

[0058]

[0059] From Eqs. (15), (16), and (17), the angular velocity Ω 1 of the three-dimensional rotating magnetic field output by the gravity-buoyancy compensator is:

[0060]

[0061] The sliding mode controller based on gravity-buoyancy compensation described in Step 3 is as follows:

[0062] Denote the desired motion trajectory of the vascular robot as [x d (t), y d (t), z d (t)] T , and assume the initial roll angular velocity of the robot Then the desired yaw angle ψ and the desired pitch angle θ of the vascular robot are respectively:

[0063]

[0064]

[0065] Assume that the roll angular velocity of the vascular robot is consistent with the rotational angular velocity Ω 1 of the three-dimensional rotating magnetic field. Then, under the action of the gravity-buoyancy compensator, the yaw angle ψ s , pitch angle θ s , and roll angular velocity of the vascular robot are:

[0066]

[0067]

[0068]

[0069] Finally, perform a dual quaternion transformation on the above-mentioned desired motion trajectory [x d (t), y d (t), z d (t)] T of the vascular robot and the attitude angles of the vascular robot after gravity-buoyancy compensation to obtain the motion description in the dual quaternion framework, so as to use the dual quaternion motion

[0070] of the desired coordinate system of the vascular robot as the input of the control system; b X b Y b Z b The control objective of the vascular robot is to make the actual motion state d X d Y d Z d of the vascular robot body coordinate system O converge asymptotically to the motion state in the desired coordinate system

[0071] Based on the kinematics and dynamics equations (3) and (4) of the vascular robot pose and trajectory integration established in Step 1, the following sliding mode surface is designed:

[0072]

[0073] where, is the product operation of dual numbers, are the controller parameters, and k si > 0, The sliding mode control law is designed as:

[0074]

[0075] where, is the sliding mode variable, sgn(·) is the sign function, and are the controller parameters, and k εi > 0, are the controller parameters, and k ui > 0, is a kind of product operation of dual vectors, is the dual interference force;

[0076] The adaptive sliding mode controller described in Step 4 is:

[0077] To facilitate the design of the adaptive sliding mode controller, the terms with M b in Equation (27) are expressed by the following formula:

[0078]

[0079] where, is another expression of the dual mass property inertia matrix M b ; and Π ∈ R 3×1 , Through the special property ε 2 = 0 of the dual dipole operation, we can further obtain is a three-dimensional dual vector; due to the particularity of the dual mass property inertia matrix M b , Π is easy to obtain, and for the calculation of , it can be derived from Equations (29)-(31): for any x ∈ R 3×1 , y ∈ R 3×1 , there is:

[0080] ​

[0081] where δ(x, y) = δ 1 (x) + δ 2 (y), and the calculation methods of δ 1 (x) and δ 2 (y) are as follows:

[0082]

[0083]

[0084] Due to the complex working environment and variable working content of the vascular robot, during the modeling process of the vascular robot, its mass and inertia often have uncertainty and variability. Therefore, for the estimated values of the dual mass characteristic inertia matrix and the dual disturbance force design the following parameter adaptive laws:

[0085]

[0086]

[0087] where is the controller parameter, is the controller parameter, and:

[0088]

[0089] Substitute equations (32) and (33) into the sliding mode control law of equation (27), then the adaptive sliding mode control law can be obtained as:

[0090]

[0091] The present invention aims at the modeling and tracking control problems of the vascular robot, establishes a kinematics and dynamics model of the posture and trajectory integration of the vascular robot based on the dual quaternion theory, and solves the problem that the posture and trajectory of the vascular robot are coupled with each other in actual operation, thus affecting the use of the vascular robot. Considering the phenomenon that the vascular robot has a sinking and offset movement under the action of gravity and buoyancy in the vertical direction, a gravity-buoyancy compensator is designed to offset the sinking effect. Based on the established model and the constructed gravity-buoyancy compensator, an adaptive sliding mode control with gravity-buoyancy compensation is designed. Finally, through simulation analysis, it is proved that the designed adaptive sliding mode controller not only has better rapidity compared with the sliding mode controller, but also can better suppress the jitter phenomenon of the sliding mode control and improve the accuracy of the posture and trajectory tracking control. It has great practical value and theoretical significance for the application of the vascular robot in minimally invasive surgery. Description of the Drawings

[0092] Figure 1 It is a schematic diagram of the coordinate system transformation represented by dual quaternions;

[0093] Figure 2 It is a schematic diagram of the sinking offset of the vascular robot due to the influence of gravity - buoyancy and after gravity - buoyancy compensation;

[0094] Figure 3 It is a schematic diagram of the physical structure and parameters of the vascular robot;

[0095] Figure 4 It is a force analysis diagram of the gravity - buoyancy compensation of the vascular robot;

[0096] Figure 5 It is a block diagram of the sliding - mode controller of the vascular robot;

[0097] Figure 6 It is a motion - trajectory tracking curve graph of the vascular robot tracking a linear reference trajectory;

[0098] Figure 7 It is a motion - trajectory error curve graph of the vascular robot tracking a linear reference trajectory;

[0099] Figure 8 It is a yaw - angle tracking curve graph of the vascular robot tracking a linear reference trajectory;

[0100] Figure 9 It is a yaw - angle tracking error graph of the vascular robot tracking a linear reference trajectory;

[0101] Figure 10 It is a pitch - angle tracking curve graph of the vascular robot tracking a linear reference trajectory;

[0102] Figure 11 It is a pitch - angle tracking error graph of the vascular robot tracking a linear reference trajectory;

[0103] Figure 12 It is a roll - angular - velocity tracking curve graph of the vascular robot tracking a linear reference trajectory;

[0104] Figure 13 It is a roll - angular - velocity tracking error graph of the vascular robot tracking a linear reference trajectory;

[0105] Figure 14 It is a motion - trajectory tracking curve graph of the vascular robot tracking a curved reference trajectory;

[0106] Figure 15 It is a motion - trajectory error curve graph of the vascular robot tracking a curved reference trajectory;

[0107] Figure 16It is the yaw angle tracking curve graph of the vascular robot tracking the curvilinear reference trajectory;

[0108] Figure 17 It is the yaw angle tracking error graph of the vascular robot tracking the curvilinear reference trajectory;

[0109] Figure 18 It is the pitch angle tracking curve graph of the vascular robot tracking the curvilinear reference trajectory;

[0110] Figure 19 It is the pitch angle tracking error graph of the vascular robot tracking the curvilinear reference trajectory;

[0111] Figure 20 It is the roll angular velocity tracking curve graph of the vascular robot tracking the curvilinear reference trajectory;

[0112] Figure 21 It is the roll angular velocity tracking error graph of the vascular robot tracking the curvilinear reference trajectory. Detailed implementation manner

[0113] The steps of the present invention are as follows:

[0114] Step 1: Establish the integrated pose and trajectory dynamics and kinematics model of the vascular robot using dual quaternions;

[0115] Step 2: Since the vascular robot will sink and deviate under the influence of gravity - buoyancy during actual movement, design a gravity - buoyancy compensator for compensation;

[0116] Step 3: Based on the vascular robot model established in Step 1 and the gravity - buoyancy compensator designed in Step 2, design a sliding - mode controller based on gravity - buoyancy compensation;

[0117] Step 4: Based on the sliding - mode controller designed in Step 3, design an adaptive sliding - mode controller.

[0118] The specific process of Step 1 is as follows:

[0119] The pose - trajectory error of the body coordinate system O of the vascular robot described by the unit dual quaternion b X b Y b Z b relative to the desired coordinate system O d X d Y d Z d is: is:

[0120]

[0121] wherein, and The desired coordinate system O of the vascular robot represented by unit dual quaternions respectively d X d Y d Z d and the body coordinate system O b X b Y b Z b The pose and trajectory relative to the inertial coordinate system OXYZ respectively. "*" represents the multiplication of dual quaternions, represents the conjugate of the dual quaternion.

[0122] The body coordinate system O of the vascular robot b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d The velocity screw error In the coordinate system O b X b Y b Z b Is expressed as:

[0123]

[0124] Among them, Is the velocity screw of the body coordinate system O of the vascular robot b X b Y b Z b Relative to the inertial coordinate system OXYZ, the representation in the body coordinate system O of the robot b X b Y b Z b In the representation. Is the velocity screw of the desired coordinate system O of the vascular robot d X d Y d Z d Relative to the inertial coordinate system OXYZ, the representation in the desired coordinate system O d X d Y d Z d (·) * Represents the conjugate of the dual quaternion.

[0125] Furthermore, through derivation, the pose and trajectory integrated kinematic and dynamic equations based on Can be obtained:

[0126]

[0127]

[0128] Among them, M b is the inertia matrix of the dual quality characteristics. represents the coordinate system O b X b Y b Z b is the dual force screw with the acting point at the centroid of the vascular robot in it. "×" represents the cross product operation of the dual quaternion, represents an operation between a matrix and a dual quaternion. For the dual quaternion define the operation of ε represents the dual unit; define the operation of is any dual quaternion.

[0129] The specific process of Step 2 is as follows:

[0130] When the vascular robot moves in the blood, define the desired motion speed of the vascular robot as The vascular robot in the blood is mainly affected by the total driving force the gravity - buoyancy of the robot fluid resistance . Under the action of gravity and buoyancy in the vertical direction, the vascular robot often sinks and deviates relative to the desired motion speed. Therefore, a gravity - buoyancy compensator is designed to enable the vascular robot to achieve the desired motion effect. The input of this compensator is the desired motion speed of the vascular robot, and the output is the angle by which the robot tilts upward relative to the ideal pitch angle and the rotational angular velocity of the three - dimensional rotating magnetic field.

[0131] The total driving force has the following expression:

[0132]

[0133] Among them, represents the driving force generated by the rotation of the spiral tail of the vascular robot under the action of the three - dimensional rotating magnetic field, is the magnetic pulling force generated by the magnetic gradient. n is the number of turns of the spiral tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, α 1 is the spiral lead angle of the tail of the vascular robot, is the rotational angular velocity of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity, is the magnetic field gradient. ζ ⊥ is the axial resistance coefficient perpendicular to the spiral tail, and ζ 11 is the axial resistance coefficient parallel to the spiral tail, and η is the blood viscosity coefficient, and κ is the diameter of the helical tail of the vascular robot.

[0134] Fluid resistance is composed of the fluid resistance generated by the impact of the blood flow on the head and helical tail of the vascular robot, and can be expressed as: Composed of, can be expressed as:

[0135]

[0136] Robot gravity - buoyancy is the resultant force of the gravity G and the buoyancy acting in the vertical direction, and its expression is:

[0137]

[0138] Among them, ρ is the density of the vascular robot, ρ f is the density of the blood, is the acceleration due to gravity.

[0139] Through the force analysis of the vascular robot, it can be seen that if we want to offset the effect of gravity - buoyancy on the vascular robot (sinking), we need the fluid resistance and the total driving force to act together, that is:

[0140]

[0141] Among them,

[0142]

[0143] To design a gravity - buoyancy compensator, an attitude coordinate system p of the vascular robot is established. The origin O of the coordinate system p is located at the center of gravity of the vascular robot, and the x - axis of the coordinate system p coincides with the actual direction of the vascular robot The z - axis p is perpendicular to the actual direction of the vascular robot is the moving speed of the vascular robot to offset the influence of gravity - buoyancy.

[0144] According to Equation (8), we can get:

[0145]

[0146] Decompose the expected moving speed of the vascular robot into a horizontal component and a vertical component It can be seen that the horizontal component of the expected moving speed of the vascular robot and The horizontal component is equal, that is, there is

[0147] The rotational angular velocity of the vascular robot and the rotational angular velocity Ω of the three-dimensional rotating magnetic field 1 have the following relationship:

[0148]

[0149] According to the above analysis, it can be obtained that:

[0150]

[0151] Among them, is a unit vector.

[0152] When , according to equations (9) and (10), it can be obtained that:

[0153]

[0154] When , decompose into two forces, that is, the component parallel to the x p axis and the component perpendicular to the x p axis (f ⊥p has no component in the direction). Also, due to the uncertainty and variability of the magnetic field gradient direction, assume acts on the x p z p plane and its direction is axisymmetric with respect to the x axis and the expected motion speed of the vascular robot p . Then, similar to the force analysis of , can be decomposed into the component parallel to the x p axis and the component perpendicular to the x p axis (f ⊥p_1 has no component in the direction), then there is:

[0155]

[0156] Among them, and respectively represent the numerical values of the projections of p on the z-axis and x p axis, and on the z pThe axis and x p The numerical value of the axis projection. Respectively represent z p The unit vectors of the axis direction and x p axis direction.

[0157] Substituting equations (10) and (13) into equation (9), we get:

[0158]

[0159] Since Define the desired pitch angle of the vascular robot as Project the velocity onto the x p axis, z p axis direction, and according to equation (14), we get:

[0160]

[0161] By analyzing the mechanical and geometric relationships of the attitude coordinate system p of the vascular robot, we get:

[0162]

[0163] where β is the angle by which the desired pitch angle of the vascular robot tilts upward.

[0164] Similar to the analysis of in equation (16), we can also obtain the following relationship:

[0165]

[0166] According to equations (15), (16) and (17), we can get:

[0167]

[0168] According to cos(θ + β) = cos(θ)cos(β) - sin(θ)sin(β), the angle β by which the vascular robot output by the gravity-buoyancy compensator tilts upward compared to the ideal pitch angle can be obtained from equation (18) as:

[0169]

[0170] From equations (15), (16) and (17), the angular velocity Ω 1 of the three-dimensional rotating magnetic field output by the gravity-buoyancy compensator is:

[0171]

[0172] The specific process of step three is as follows:

[0173] Let the expected motion trajectory of the vascular robot be [x d (t), y d (t), z d (t)] T , and let the initial roll angular velocity of the robot be Then the expected yaw angle ψ and the expected pitch angle θ of the vascular robot are respectively:

[0174]

[0175]

[0176] Assume that the roll angular velocity of the vascular robot is consistent with the rotational angular velocity Ω 1 of the three-dimensional rotating magnetic field. Then, under the action of the gravity-buoyancy compensator, the yaw angle ψ s , pitch angle θ s , and roll angular velocity of the vascular robot are:

[0177]

[0178]

[0179]

[0180] Finally, perform a dual quaternion transformation on the above-mentioned expected motion trajectory [x d (t), y d (t), z d (t)] T of the vascular robot and the attitude angles of the vascular robot after gravity-buoyancy compensation to obtain the motion description in the dual quaternion framework, so as to use the dual quaternion motion

[0181] of the expected coordinate system of the vascular robot as the input of the control system. b X b Y b Z b The control objective of the vascular robot is: by controlling the pose and trajectory error d X d Y d Z d of the vascular robot's body coordinate system O relative to the expected coordinate system O to make the actual motion state

[0182] Based on the kinematics and dynamics equations (3) and (4) of the vascular robot's pose and trajectory integration established in Step 1, the following sliding mode surface is designed:

[0183]

[0184] Wherein, is the product operation of dual numbers. are the controller parameters, and k si > 0,

[0185] The sliding mode control law is designed as:

[0186]

[0187] Wherein, is the sliding mode variable, sgn(·) is the sign function, and are the controller parameters, and k εi > 0, are the controller parameters, and k ui > 0, is a kind of product operation of dual vectors. is the dual disturbance force.

[0188] The specific process of Step Four is as follows:

[0189] There is a jitter phenomenon in the sliding mode controller designed in Step Three. Moreover, during the clinical medical application process of the vascular robot, it often needs to face different working tasks and working environments. Therefore, the model parameters of the vascular robot have unestimable change characteristics and uncertainties (mainly reflected in the inaccuracy of the dual mass characteristic inertia matrix M b and the uncertainty of the dual disturbance force ). Therefore, to solve the problems of uncertain and inaccurate model parameters and suppress the jitter problem in the sliding mode controller, an adaptive sliding mode controller is designed to improve the control performance and control accuracy of the vascular robot system.

[0190] To facilitate the design of the adaptive sliding mode controller, the term with M b in Equation (27) is expressed by the following formula:

[0191]

[0192] Wherein, is the dual mass characteristic inertia matrix Mb Another expression; and Π ∈ R 3×1 , Through the special property ε of the dipole operation 2 = 0, we can further obtain is a three-dimensional dual vector; Since the dual mass characteristic inertia matrix M b is special, Π is easily obtained, and regarding 's calculation, it can be derived from equations (29)-(31): For any x ∈ R 3×1 , y ∈ R 3×1 , there is:

[0193]

[0194] where δ(x,y) = δ 1 (x) + δ 2 (y), and the calculation methods of δ 1 (x), δ 2 (y) are as follows:

[0195]

[0196]

[0197] Due to the complex working environment and variable working content of the vascular robot, during the modeling process of the vascular robot, its mass and inertia often have uncertainty and variability. Therefore, for the estimated values of the dual mass characteristic inertia matrix and the dual disturbance force , the design parameter adaptive law is as follows: Design parameter adaptive law is as follows:

[0198]

[0199]

[0200] where, is the controller parameter, is the controller parameter, and:

[0201]

[0202] Substitute equations (32) and (33) into the sliding mode control law of equation (27), then the adaptive sliding mode control law can be obtained as:

[0203]

[0204] The following further elaborates on the present invention in conjunction with the attached drawings

[0205] First, basic explanations of quaternions and dual quaternions are given.

[0206] Quaternion

[0207] A quaternion is a mathematical theory proposed by Hamilton in 1843, and its form is:

[0208]

[0209] where \(l\) 0 is a real number, representing the scalar part of the quaternion; represents the vector part of the quaternion; \(l\) v1 , \(l\) v2 , \(l\) v3 are real numbers, are all orthogonal vectors, and

[0210] The basic operation rules of quaternions are as follows:

[0211]

[0212] where \(l\) 1 , \(l\) 2 represent quaternions; \(\lambda\) is a real number; \(l\) * represents the conjugate of quaternion \(l\); \(\|l\|\) represents the modulus of quaternion \(l\), and a quaternion with modulus 1 is defined as a unit quaternion; \(l\) -1 represents the inverse of quaternion \(l\); \(\text{vec}(l)\) represents the operation of converting quaternion \(l\) into a vector, that is, the first element of \(l\) is taken as 0 to make \(l\) a vector.

[0213] For convenient calculation, the operation of matrix and quaternion is defined as:

[0214]

[0215] where is a fourth-order matrix.

[0216] “*” represents the multiplication of quaternions, and the specific calculation is:

[0217]

[0218] where E 3 represents the third-order identity matrix, and is the cross-product matrix.

[0219] Through derivation, it can be obtained that:

[0220]

[0221] Specifically, under and conditions, the following calculations are defined respectively:

[0222]

[0223]

[0224] Define the "×" operation of quaternion as:

[0225]

[0226] Dual number

[0227] The dual number was proposed by mathematician Clifford and is defined as:

[0228]

[0229] where a, are real numbers and are the real part and the dual part respectively; ε represents the dual unit, ε 2 = 0 and ε ≠ 0.

[0230] The dual number has the following operations:

[0231]

[0232] where, are all dual numbers; μ is a real number.

[0233] For the convenience of calculation, define the operation of the dual number as:

[0234]

[0235] Dual vector

[0236] When both the real part and the dual part of the dual number are three-dimensional vectors, this dual number is defined as a dual vector, that is where When the real part in the dual vector is a free vector and the dual part is a bound vector (that is, the vector in the real part is independent of the selected reference point, while the vector in the dual part is related to the selected reference point), this dual vector is called a spinor. The basic operations of the dual vector are as follows:

[0237] Define the operation "<>" for taking the absolute value of the dual vector:

[0238]

[0239] It can be seen from the expression of Equation (1012) that "<>" represents a dual vector with all positive elements.

[0240] Definition is a product operation of dual vectors, and its specific calculation is as follows:

[0241]

[0242] Furthermore, through calculation, we can obtain:

[0243]

[0244] As can be seen from Equation (1014), after the operation, the resulting value is a new dual vector.

[0245] Define the inner product operation of dual vectors, with the symbol "||·||", and the specific calculation is shown in the following formula:

[0246]

[0247] As can be seen from the above formula, the result of the inner product operation of dual vectors is a real number.

[0248] Dual momentum

[0249] Define that when the center of mass E of the rigid body c is the reference point of the dual momentum, the dual momentum of the rigid body can be expressed as:

[0250]

[0251] where m is the mass of the rigid body, is the velocity of the rigid body, represents the linear momentum of the rigid body with the center of mass E c as the reference point, and Λ E represents the inertia matrix of the center of mass E c in the body coordinate system, is the angular velocity of the rigid body, represents the angular momentum of the rigid body with the center of mass E c regarded as the reference point.

[0252] Differentiate Equation (1016), then we get the Newton-Euler rigid body dynamics equation at the center of mass E c :

[0253]

[0254] where, is the dual force screw, and are the external force and torque acting on the center of mass respectively.

[0255] Dual quaternion

[0256] A dual quaternion is a quaternion with each element being a dual number or a dual number with a quaternion as the real part and the dual part, defined as:

[0257]

[0258] where \(l\) and represent any quaternion; \(\epsilon\) represents a dual number, \(\xi\) represents a dual vector;

[0259] According to the definition of the dual quaternion, it can be seen that the basic operation rules of the dual quaternion and the quaternion are similar, which will not be elaborated here.

[0260] For the convenience of later analysis and understanding, the matrix and dual quaternion operations are defined in the following form:

[0261]

[0262] where, is an eight-order matrix.

[0263] For the convenience of later analysis and understanding, similar to the operations defined by equations (1006) and (1007) in quaternions, the operations of and are also defined similarly in dual quaternions.

[0264] Establish the integrated dynamics and kinematics model of the pose and trajectory of the vascular robot using dual quaternions:

[0265] Define the body coordinate system of the vascular robot as \(O\) b \(X\) b \(Y\) b \(Z\) b , the inertial coordinate system as \(OXYZ\), and the desired coordinate system as \(O\) d \(X\) d \(Y\) d \(Z\) d .

[0266] The attitude kinematics equation of the vascular robot based on unit quaternions: First, describe the three-dimensional rotational motion of the vascular robot using unit quaternions.

[0267] According to Euler's theorem, when a rigid body rotates about a fixed point, it can be understood that the rigid body rotates a certain angle about a certain straight-line axis passing through the fixed point. Furthermore, the motion description of the coordinate system \(n\) rotating by an angle \(\alpha\) about the axis to obtain the coordinate system \(m\) can be obtained as:

[0268]

[0269] where, Therefore, l mn is a unit quaternion. And

[0270] Since the inertial coordinate system OXYZ is the reference fixed coordinate system during the movement of the vascular robot, the attitude kinematic equation of the body coordinate system O b X b Y b Z b of the vascular robot relative to the inertial coordinate system OXYZ is:

[0271]

[0272] Wherein, are respectively the representations of the angular velocity of the body coordinate system O b X b Y b Z b of the vascular robot relative to the inertial coordinate system OXYZ in the body coordinate system O b X b Y b Z b and the inertial coordinate system OXYZ.

[0273] Similarly, the attitude kinematic equation of the desired coordinate system O d X d Y d Z d of the vascular robot relative to the inertial coordinate system OXYZ is:

[0274]

[0275] Wherein, are respectively the representations of the angular velocity of the desired coordinate system O d X d Y d Z d of the vascular robot relative to the inertial coordinate system OXYZ in the desired coordinate system O d X d Y d Z d and the inertial coordinate system OXYZ.

[0276] Since the unit quaternion has the property of multiplicative error, the attitude error l b X b Y b Z b of the body coordinate system O of the vascular robot relative to the desired coordinate system O d X d Y d Z d is le is:

[0277]

[0278] The body coordinate system O of the vascular robot b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d The angular velocity, i.e., the angular velocity error In the coordinate system O b X b Y b Z b is expressed as:

[0279]

[0280] where,

[0281] By performing differential operations on Equation (1) and substituting Equations (1021), (1022), and (1024), it can be obtained that the kinematic equation of the attitude error of the body coordinate system O of the vascular robot b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d is:

[0282]

[0283] where, is the error angular velocity of the body coordinate system O of the vascular robot b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d in the coordinate system O d X d Y d Z d representation.

[0284] Kinematic equations for integrated posture and trajectory of vascular robots based on dual quaternions: The above-mentioned unit quaternion only describes the three-dimensional rotational motion (i.e., posture motion) of vascular robots, while dual quaternions can represent rotational and translational motions in the same mathematical expression. Therefore, dual quaternions are used to describe the three-dimensional rotational and translational motions (i.e., integrated posture and trajectory motion) of vascular robots.

[0285] According to Chasles' theorem, screw motion can be used to describe the general motion of a rigid body, and the six-degree-of-freedom rotational and translational motions of the rigid body in its body coordinate system can be described simultaneously using dual quaternions. Figure 1 It describes the process of the W coordinate system first rotating by an angle α around the rotation axis and then translating a distance d” along the line direction to the K coordinate system. This process is represented by the unit dual quaternion as:

[0286]

[0287] where

[0288] Suppose there is a straight line in space, which is represented as and in the W coordinate system and the K coordinate system respectively. According to Equation (6), we can obtain Furthermore, we get:

[0289]

[0290] where is the representation of the position of the K coordinate system relative to the W coordinate system in the K coordinate system. Then, the motion of the K coordinate system relative to the W coordinate system can be described as:

[0291]

[0292] where is the representation of the position of the K coordinate system relative to the W coordinate system in the W coordinate system.

[0293] Combining Equation (1025) and taking the derivative of Equation (1028), we can obtain the kinematic equations for integrated posture and trajectory described using dual quaternion theory:

[0294]

[0295] where is the representation of the velocity screw of the K coordinate system relative to the W coordinate system in the K coordinate system, is the representation of the velocity screw of the K coordinate system relative to the W coordinate system in the W coordinate system.

[0296] Combining Equation (1021) and Equation (1029), the kinematic equation of the vascular robot's body coordinate system O b X b Y b Z b relative to the inertial coordinate system OXYZ, that is, the integrated pose and trajectory kinematic equation of the vascular robot in the inertial coordinate system is:

[0297]

[0298] where, are respectively the representations of the velocity screw of the vascular robot's body coordinate system O b X b Y b Z b relative to the inertial coordinate system OXYZ in the robot's body coordinate system O b X b Y b Z b and the inertial coordinate system OXYZ.

[0299] Similarly, combining Equation (1022) and Equation (1029), the integrated pose and trajectory kinematic equation of the dual quaternion theory describing the inertial coordinate system OXYZ of the vascular robot relative to the desired coordinate system O d X d Y d Z d is:

[0300]

[0301] where, are respectively the representations of the velocity screw of the desired coordinate system O of the vascular robot d X d Y d Z d relative to the inertial coordinate system OXYZ in the desired coordinate system O d X d Y d Z d and the inertial coordinate system OXYZ.

[0302] The unit dual quaternion has a multiplicative error property similar to that of the unit quaternion. Then, the pose and trajectory error b X b Y b Z b of the vascular robot's body coordinate system O d X d Y d Z d relative to the desired coordinate system O is:

[0303]

[0304] According to equations (1025) and (1029), the vascular robot body coordinate system O in the dual quaternion framework can be obtained: b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d The attitude-orbit integrated kinematic equation is:

[0305]

[0306] in, and is the vascular robot body coordinate system O b X b Y b Z b Relative to the expected calibration system O d X d Y d Z d The velocity screw error in the coordinate system O b X b Y b Z b and coordinate system O d X d Y d Z d The following expression.

[0307] The integrated dynamic equation of the vascular robot's attitude and trajectory based on dual quaternion:

[0308] For the convenience of analysis, it is assumed that the vascular robot is a rigid body. According to formula (1017), the dynamic equation of the vascular robot with the center of mass as the reference point can be obtained:

[0309]

[0310] in, Represents the coordinate system O b X b Y b Z b The dual force spinor whose action point is the center of mass of the vascular robot, In the coordinate system O b X b Y b Z b The resultant force of the point of action on the center of mass of the vascular robot is represents the total driving force, represents the gravity - buoyancy force of the robot represents the fluid resistance represents the dual interference force

[0311] represents in the coordinate system O b X b Y b Z b the resultant moment acting on the centroid of the vascular robot, where respectively represent the driving moment, the gravity - buoyancy moment of the robot, the fluid resistance moment and the dual interference moment is the dual momentum with the centroid of the vascular robot as the reference point

[0312] Define the dual mass - property inertia matrix as:

[0313]

[0314] where, Λ b is the inertia matrix of the vascular robot's rotation in the body coordinate system O b X b Y b Z b and m is the mass of the vascular robot

[0315] Define Combining Equation (1016) and Equation (1019), we have:

[0316]

[0317] Deriving Equation (1034) according to Equation (1036), we can get:

[0318]

[0319] Based on the angular velocity error represented by quaternion in Equation (1024), we can get the velocity screw error of the body coordinate system O b X b Y b Z b of the vascular robot with respect to the desired coordinate system O b X b Y b Z b of the vascular robot as: d X d Y d Z d is:

[0320]

[0321] Derive the derivative of Equation (1038), and then it can be deduced from Equation (1027) and Equation (1033) that:

[0322]

[0323] Substitute Equation (1037) into Equation (1039), and after further derivation, the integrated kinematic and dynamic equation of the vascular robot's body coordinate system O b X b Y b Z b relative to the desired coordinate system O d X d Y d Z d is:

[0324]

[0325] In summary, the integrated kinematic and dynamic equations of the vascular robot established based on dual quaternions are:

[0326]

[0327]

[0328] Design the gravity-buoyancy compensator of the vascular robot:

[0329] When the vascular robot moves in the blood, to meet the working requirement that the vascular robot can move back and forth in the blood vessel, assume that the vascular robot moves against the blood flow velocity i.e., the desired motion velocity of the vascular robot is in the opposite direction to the blood flow velocity , and is the actual motion velocity of the vascular robot. The vascular robot in the blood is mainly affected by the following forces: the total driving force the gravity-buoyancy of the robot the fluid resistance (Since the electrostatic force and van der Waals force acting on the vascular robot are very small, they are ignored here). Under the action of the gravity-buoyancy in the vertical direction, the vascular robot often shows a phenomenon of sinking and offset relative to the desired motion velocity. Therefore, a gravity-buoyancy compensator is designed, with the input being the desired motion velocity of the vascular robot and the output being the angle by which the robot tilts upward relative to the ideal pitch angle and the rotational angular velocity of the three-dimensional rotating magnetic field. That is, on the basis of the traditional control strategy, the vascular robot is made to cooperate with the rotational angular velocity of the magnetic field to tilt upward by a certain angle relative to the traditional desired posture under different desired motion trajectory requirements, so that the vascular robot achieves the desired motion effect. The schematic diagram of the robot sinking and offset and after gravity-buoyancy compensation is shown in Figure 2 as shown.

[0330] Total driving force The expression is as follows:

[0331]

[0332] Wherein, represents the driving force generated by the rotation of the helical tail of the vascular robot under the action of a three-dimensional rotating magnetic field, is the magnetic pulling force generated by the magnetic gradient. n is the number of rotation turns of the helical tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, α 1 is the helical pitch angle of the tail of the vascular robot, ζ ⊥ is the axial resistance coefficient perpendicular to the helical tail, ζ 11 is the axial resistance coefficient parallel to the helical tail. is the angular velocity of rotation of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity, is the magnetic field gradient.

[0333] Total driving force The calculation of requires obtaining the values of ζ ⊥ and ζ 11 These two parameters, and they are respectively:

[0334]

[0335]

[0336] Wherein, η is the blood viscosity coefficient, κ is the diameter of the helical tail of the vascular robot. The schematic diagram of the physical structure and parameters of the vascular robot is as shown in Figure 3 shown.

[0337] Fluid resistance The fluid resistance generated by the impact of the blood flow on the head and helical tail of the vascular robot is composed of, and can be expressed as:

[0338]

[0339] Robot gravity - buoyancy is the resultant force of the gravity G and the buoyancy acting in the vertical direction, and its expression is:

[0340]

[0341] f where ρ is the density of the vascular robot, ρ is the density of the blood,

[0342] Figure 2 Through Figure 2From the force analysis of the vascular robot, it can be seen that if we want to offset the effect of gravity - buoyancy on the vascular robot (sinking), fluid resistance and the total driving force are required to act together, that is:

[0343]

[0344] Among them,

[0345]

[0346] To design a gravity - buoyancy compensator, an attitude coordinate system p of the vascular robot is established, as Figure 4 shown. The origin O of the coordinate system p is located at the center of gravity of the vascular robot, and the x - axis of the coordinate system p coincides with the actual direction of the vascular robot The z - axis p is perpendicular to the actual direction of the vascular robot is the expected motion speed of the vascular robot, is the motion speed of the vascular robot to offset the influence of gravity - buoyancy.

[0347] According to Equation (1048), we can get:

[0348]

[0349] From Figure 4 it can be seen that by decomposing the expected motion speed of the vascular robot into a horizontal component and a vertical component It is easy to know that the horizontal component of the expected motion speed of the vascular robot is equal to the horizontal component of That is, there is

[0350] The rotational angular velocity of the vascular robot and the rotational angular velocity Ω of the three - dimensional rotating magnetic field 1 have the following relationship:

[0351]

[0352] Then from Figure 4 we can get:

[0353]

[0354] Among them, is a unit vector.

[0355] When When, according to Figure 4 and Equation (49), Equation (50) can be obtained as follows:

[0356]

[0357] When When, decompose into two forces, namely the component p parallel to the x axis and the component p perpendicular to the x axis. It is easy to know that f ⊥p has no component in the direction, so there is:

[0358]

[0359] Among them, respectively represent the numerical values of the projections on the z p axis and the x p axis; respectively represent the unit vectors in the z p axis direction and the x p axis direction.

[0360] Due to the uncertainty and variability of the magnetic field gradient direction, assume acts on the x p -z p plane and the direction is axisymmetric with respect to the expected motion speed of the vascular robot about the x p axis. Then, similar to the force analysis, can be decomposed into the component p parallel to the x axis and the component p perpendicular to the x axis. It is easy to know that f ⊥p_1 has no component in the direction, so there is:

[0361]

[0362] Among them, respectively represent the numerical values of the projections on the z p axis and the x p axis.

[0363] Substitute Equation (1050), Equation (1053) and Equation (1054) into Equation (1049), and we can get:

[0364]

[0365] Since Define Figure 4 The desired pitch angle of the vascular robot in Project the velocity onto the x p axis and the z p axis directions, and obtain according to Equation (55):

[0366]

[0367] By analyzing Figure 4 the mechanical and geometric relationships of

[0368]

[0369] where β is the upward tilt angle of the desired pitch angle of the vascular robot.

[0370] Similar to Figure 4 the analysis for in Equation (1057), the following relationship can also be obtained:[[]]

[0371]

[0372] According to Equation (1056), Equation (1057), and Equation (1058), it can be obtained that:[[]]

[0373]

[0374] Since cos(θ + β) = cos(θ)cos(β) - sin(θ)sin(β), then from Equation (1059), the angle β by which the vascular robot output by the gravity-buoyancy compensator tilts upward compared to the ideal pitch angle is:[[]]

[0375]

[0376] Through Equation (1056), Equation (1057), and Equation (1058), it can be obtained that: the angular velocity Ω of the three-dimensional rotating magnetic field output by the gravity-buoyancy compensator 1 is:[[]]

[0377]

[0378] Design a sliding mode controller for the vascular robot based on gravity-buoyancy compensation:[[]]

[0379] The sliding mode control block diagram of the vascular robot is as Figure 5 shown.[[]]

[0380] Denote the desired motion trajectory of the vascular robot as [x d (t), y d (t), zd (t)] T Let the initial rolling angular velocity of the robot be Then the desired yaw angle ψ and the desired pitch angle θ of the vascular robot are respectively:

[0381]

[0382]

[0383] Assume the rolling angular velocity of the vascular robot is is consistent with the rotational angular velocity Ω of the three-dimensional rotating magnetic field 1 Then, under the action of the gravity-buoyancy compensator, the yaw angle ψ of the vascular robot s , pitch angle θ s , rolling angular velocity are:

[0384]

[0385]

[0386]

[0387] Finally, perform a dual quaternion transformation on the above desired motion trajectory [x d (t), y d (t), z d (t)] T of the vascular robot and the attitude angle of the vascular robot after gravity-buoyancy compensation to obtain the motion description in the dual quaternion framework Thus, the dual quaternion motion of the desired coordinate system of the vascular robot is used as the input of the control system.

[0388] The control objective of the vascular robot is: By controlling the pose and trajectory error b X b Y b Z b of the vascular robot body coordinate system O relative to the desired coordinate system O d X d Y d Z d to make the actual motion state of the vascular robot asymptotically converge to the motion state

[0389] Based on the pose and trajectory integrated kinematic and dynamic equations (1041) and (1042) established in step 1, design the following sliding surface:

[0390]

[0391] Among them, The operation of is shown in Equation (1011). is the controller parameter, and k si > 0,

[0392] The sliding mode control law is designed as:

[0393]

[0394] Among them, is the sliding mode variable, sgn(·) is the sign function, and is the controller parameter, and k εi > 0, is the controller parameter, and k ui > 0, and the operation regarding is shown in Equation (1013).

[0395] Proof of the stability of the sliding mode controller

[0396] For the non-linear system of the vascular robot, the proof of the stability of the sliding mode controller mainly has two steps: First, prove that the control system can converge to at any initial state, and then further prove the stability of the closed-loop control system on the sliding mode surface .

[0397] The following is the specific proof process:

[0398] Step 1: Prove that the control system can converge to at any initial state. Design the following Lyapunov function:

[0399]

[0400] Among them, the calculation of "||·||" is shown in Equation (1015).

[0401] Since M b is a positive definite symmetric matrix, then V 1 ≥ 0, and V = 0 if and only if 1 ; Take the derivative of the above formula and substitute Equation (67) into it, and we can get:

[0402]

[0403] Among them,

[0404] Substituting the integrated kinematic and dynamic equation (1042) of the vascular robot into Equation (1070), we can obtain:

[0405]

[0406] Substituting Equation (1068) into Equation (1071), we can get:

[0407]

[0408] Among them, the specific calculation of <·> can be seen in Equation (1012).

[0409] Step 2: Prove the stability of the closed-loop control system on the sliding surface above.

[0410] Define the following Lyapunov function:

[0411]

[0412] It is easy to know that V 2 ≥0, and V is only equal to 0 when 2 ; Differentiate Equation (1073):

[0413]

[0414] Furthermore, through calculation, we have:

[0415]

[0416] After deriving the term in Equation (1075), we can obtain:

[0417]

[0418] Continuing to derive (1076), we can get:

[0419]

[0420] Similarly, for the other two terms in Equation (1075), the derivation process is similar to that of Equations (1076) and (1077), and will not be elaborated here.

[0421] According to the analysis of the derivation processes of Equations (1076) and (1077), Equation (1075) can be written as:

[0422]

[0423] When When, it can be obtained that Substituting into Equation (1078), it can be obtained that:

[0424]

[0425] Therefore, it is proved.

[0426] Step 4: Design the adaptive sliding mode controller of the vascular robot:

[0427] There is a jitter phenomenon in the sliding mode controller designed in Step 3. Moreover, during the clinical medical application process of the vascular robot, it often needs to face differential working tasks and working environments. Therefore, the model parameters of the vascular robot have characteristics of unestimable changes and uncertainties (mainly reflected in the inaccuracy of the dual mass characteristic inertia matrix M b and the uncertainty of the dual disturbance force ). Therefore, to solve the problems of uncertain and inaccurate model parameters and suppress the jitter problem in the sliding mode controller, an adaptive sliding mode controller is designed to improve the control performance and control accuracy of the vascular robot system.

[0428] To facilitate the design of the adaptive sliding mode controller, the term with M b in Equation (68) is expressed by the following formula:

[0429]

[0430] Where is another expression of the dual mass characteristic inertia matrix M b ; and Π ∈ R 3×1 , Through the special property ε 2 = 0 of the dual dipole operation, it can be further obtained that is a three-dimensional dual vector; due to the particularity of the dual mass characteristic inertia matrix M b , Π can be easily obtained. For the calculation of , it can be deduced from Equations (1081)-(1083): For any x ∈ R 3×1 , y ∈ R 3×1 , there is:

[0431]

[0432] Where δ(x, y) = δ 1 (x) + δ 2 (y), and the calculation methods of δ 1 (x) and δ 2 (y) are as follows:

[0433]

[0434]

[0435] Due to the complex working environment and variable working content of the vascular robot, during the modeling process of the vascular robot, its mass and inertia often have uncertainty and variability. Therefore, for the dual mass characteristic inertia matrix and the dual disturbance force the estimated value of the adaptive law of the design parameters is as follows:

[0436]

[0437]

[0438] where is the controller parameter, is the controller parameter, and:

[0439] Substituting Eqs. (1084) and (1085) into the sliding mode control law of Eq. (1068), the adaptive sliding mode control law can be obtained as:

[0440]

[0441] Proof of the stability of the adaptive sliding mode controller

[0442] Since the stability of the sliding mode controller of the vascular robot has been proven in step 3 therefore, in this step, it is only necessary to prove that the control system can converge to the sliding mode surface under the action of the adaptive law. Define the following Lyapunov function:

[0443]

[0444] where and there is is the estimation error, and define Therefore, for any there is V a ≥0, and V = 0 if and only if a = 0.

[0445] Taking the derivative of the above Lyapunov function with respect to time t, we have:

[0446]

[0447] According to Eqs. (70) and (71) in step 3, we know that:

[0448]

[0449] Substituting the integrated kinematic and dynamic equation (1042) of the vascular robot into equation (1090), we get:

[0450]

[0451] Furthermore, substituting the adaptive sliding mode control law equation (1087) into equation (1091), we get:

[0452]

[0453] Substituting the above equation into equation (1089), we get:

[0454]

[0455] Since the various mass coefficients of the vascular robot and the disturbances it is subjected to change relatively slowly, we have: Furthermore, we can obtain:

[0456]

[0457]

[0458] Deriving the term in equation (1093), we get:

[0459]

[0460] Combining equation (1086), equation (1087) and equation (1096), and substituting them into equation (1093), we get:

[0461]

[0462] Thus, it is proven.

[0463] The following specific embodiments are used to verify the beneficial effects of the present invention.

[0464] Embodiment 1: Use the adaptive sliding mode controller designed in step 4 to track the linear motion trajectory of the vascular robot, and compare it with the sliding mode controller designed in step 3 to verify the control effect of the adaptive sliding mode controller.

[0465] Vascular robot simulation parameter settings:

[0466] Table 1 Vascular robot simulation parameters

[0467]

[0468]

[0469] Select the starting point of the vascular robot as the coordinate origin, and design the following linear reference trajectory curve:

[0470]

[0471] Use the gravity-buoyancy compensator for attitude compensation, and perform simulation through the adaptive sliding mode control method. The controller parameters are:

[0472]

[0473]

[0474]

[0475]

[0476]

[0477] Through simulation, the simulation results of the vascular robot tracking the linear motion trajectory under the action of the sliding mode controller and the adaptive sliding mode controller are obtained, as Figures 6 - 13 shown. From Figure 6 , Figure 7 it can be seen that under the action of sliding mode control and adaptive sliding mode control, the trajectory control of the vascular robot has good tracking accuracy, and the adaptive sliding mode control is obviously better than the sliding mode control, mainly manifested in that the adaptive sliding mode control can meet higher control accuracy requirements; and compared with the sliding mode control, the adaptive sliding mode control saves most of the error convergence adjustment time in the trajectory control of the vascular robot and has higher system response rapidity. Figures 8 - 13 is the attitude control result of the vascular robot. It can be seen from the figure that compared with the sliding mode control, the adaptive sliding mode control has higher attitude tracking control accuracy and shorter adjustment time. In addition, from the attitude angle errors Figure 9 , Figure 11 , Figure 13 it can be seen that the adaptive sliding mode control has better anti-jitter performance than the sliding mode control, which helps to improve the stability of the control system.

[0478] Example 2: Use the adaptive sliding mode controller designed in step 4 to track the curved motion trajectory of the vascular robot, and compare it with the sliding mode controller designed in step 3 to verify the control effect of the adaptive sliding mode controller.

[0479] The settings of the robot simulation parameters are the same as those in Example 1.

[0480] In order to verify the effectiveness of the adaptive sliding mode controller under different working conditions and cope with the changing working conditions, the curved motion trajectory is tracked in this example.

[0481] Let the starting point of the vascular robot be the coordinate origin, and design the motion trajectory curve of the vascular robot:

[0482]

[0483] Use the gravity-buoyancy compensator for attitude compensation, and conduct simulation through the adaptive sliding mode control method. The controller parameters are set as:

[0484]

[0485]

[0486]

[0487]

[0488]

[0489] Through simulation, the simulation results of the vascular robot tracking the curve-shaped motion trajectory under the action of the sliding mode controller and the adaptive sliding mode controller are obtained, as Figures 14 - 21 shown. Figures 14 - 15 is the motion trajectory result diagram of the vascular robot under the action of the sliding mode controller and the adaptive sliding mode controller. It can be seen from the figure that compared with the sliding mode control, the adaptive sliding mode control saves most of the error convergence adjustment time in the trajectory control of the vascular robot, and the system response speed is faster. Figures 16 - 21 is the attitude control result of the vascular robot. It can be seen from the figure that the attitude tracking error of the adaptive sliding mode control is much smaller than that of the sliding mode control, and the adjustment time is short. It verifies that the proposed adaptive sliding mode control method has higher attitude tracking control accuracy. In addition, from the attitude angle errors Figure 17 , Figure 19 and Figure 21 it can be seen that the adaptive sliding mode control has better anti-jitter performance than the sliding mode control, improving the stability of the control system.

Claims

1. A posture and trajectory integrated adaptive sliding mode tracking control method for a helical vascular robot, characterized in that: The steps are as follows: Step 1: Use dual quaternions to describe the posture and trajectory motion of the helical vascular robot simultaneously, and establish the kinematic and dynamic models of the posture and trajectory integration of the helical vascular robot; Step 2: When the helical vascular robot moves in the blood, it will be affected by gravity - buoyancy, resulting in the actual motion trajectory sinking and deviating, and deviating from the desired trajectory. Design a gravity - buoyancy compensator to compensate for the sinking effect of gravity - buoyancy in the vertical direction; Step 3: Based on the helical vascular robot model established in Step 1 and the gravity - buoyancy compensator designed in Step 2, design a sliding mode controller based on gravity - buoyancy compensation; Step 4: Based on the sliding mode controller designed in Step 3, design an adaptive sliding mode controller; The specific process of establishing the kinematic and dynamic models of the posture and trajectory integration described in Step 1 is as follows: The body coordinate system O of the vascular robot described by unit dual quaternion b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d The pose and trajectory error is as follows: Among them, and are the pose-trajectories of the vascular robot's desired coordinate system \(O_{X}Y_{Z}\) and the body coordinate system \(O_{x}y_{z}\) represented by unit dual quaternions respectively, where the "*" represents the multiplication of dual quaternions, d X d Y d Z d and b X b Y b Z b with respect to the inertial coordinate system \(OXYZ\) respectively, and "\*" represents the conjugate of the dual quaternion; represents the conjugate of the dual quaternion; Vascular robot body coordinate system O b X b Y b Z b Relative to the desired coordinate system O d X d Y d Z d The velocity screw error In the coordinate system O b X b Y b Z b Is expressed as: Among them, is the body coordinate system O of the vascular robot b X b Y b Z b The screw of the velocity of the body coordinate system O of the vascular robot relative to the inertial coordinate system OXYZ b X b Y b Z b representation in is the desired coordinate system O of the vascular robot d X d Y d Z d The screw of the velocity of the desired coordinate system O of the vascular robot relative to the inertial coordinate system OXYZ d X d Y d Z d representation in, (·) * represents the conjugate of the dual quaternion; Furthermore, through derivation, the kinematic and dynamic equations of attitude and orbit integration based on can be obtained: Among them, M b is the inertia matrix of the dual mass characteristics, represents the coordinate system O b X b Y b Z b in which the acting point is the dual force screw of the centroid of the vascular robot, and "×" represents the cross product operation of the dual quaternion, represents an operation between a matrix and a dual quaternion. For the dual quaternion define the operation of ε represents the dual unit; define the operation of is any dual quaternion; The specific design process of the gravity - buoyancy compensator described in Step 2 is as follows: When the vascular robot moves in the blood, the expected movement speed of the vascular robot is defined as The vascular robot in the blood is mainly affected by the total driving force The gravity - buoyancy of the robot Fluid resistance ; The total driving force The expression of is: Among them, represents the driving force generated by the rotation of the helical tail of the vascular robot under the action of a three-dimensional rotating magnetic field. is the magnetic pulling force generated by the magnetic gradient, n is the number of rotation turns of the helical tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, and α 1 is the helical pitch angle of the tail of the vascular robot. is the angular velocity of rotation of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity. is the magnetic field gradient, and ζ ⊥ is the axial resistance coefficient perpendicular to the helical tail, and ζ 11 is the axial resistance coefficient parallel to the helical tail, and η is the blood viscosity coefficient, and κ is the diameter of the helical tail of the vascular robot. Fluid resistance The fluid resistance generated by the impact of blood flow on the head and spiral tail of the vascular robot is composed of, expressed as: Robot gravity - buoyancy is the resultant force of gravity G and buoyancy acting in the vertical direction, and its expression is: where ρ is the density of the vascular robot, and ρ f is the density of blood, and is the acceleration due to gravity; To offset the effects of gravity and buoyancy on the vascular robot, fluid resistance and the total driving force are required to act together, that is: Among them To design a gravity-buoyancy compensator, an attitude coordinate system p of the vascular robot is established, and the origin O of the coordinate system p is located at the center of gravity of the vascular robot, and the x-axis of the coordinate system p coincides with the actual direction of the vascular robot and the z-axis p is perpendicular to the actual direction of the vascular robot is the movement speed of the vascular robot to counteract the influence of gravity and buoyancy According to Equation (8), we get: Decompose the expected motion speed of the vascular robot into horizontal components and vertical components It can be known that the horizontal component of the expected motion speed of the vascular robot is equal to the horizontal component of and That is, there is The angular velocity of rotation of the vascular robot and the angular velocity of rotation Ω of the three-dimensional rotating magnetic field 1 have the following relationship:​ According to the above analysis, we can obtain: Among them, is a unit vector; When , according to equations (9) and (10), we can obtain: When it is decomposed into two forces, namely the component parallel to the x axis p and the component perpendicular to the x axis p There is no component in the direction. Also, due to the uncertainty and variability of the magnetic field gradient direction, assume that acts on the x p zplane and its direction is axisymmetric with respect to the expected motion speed of the vascular robot p about the x p axis. Then, similar to the force analysis of it can be decomposed into the component parallel to the x p axis and the component perpendicular to the x p axis There is no component in the direction, so we have: Among them, and respectively represent the numerical values of the projections on the z p axis and the x p axis, and the numerical values of the projections on the z p axis and the x p axis. They respectively represent the unit vectors in the z p axis direction and the x p axis direction; Substituting Eqs. (10) and (13) into Eq. (9), we can obtain: Since Define the desired pitch angle of the vascular robot as Project the velocity onto the x p axis and the z p axis directions, and according to Equation (14), we can obtain: By analyzing the mechanical and geometric relationships of the posture coordinate system p of the vascular robot, we can obtain: where β is the angle of upward inclination of the desired pitch angle of the vascular robot; Similar to the analysis for in Equation (16), the following relationship can also be obtained: According to Equation (15), Equation (16) and Equation (17), we get: According to cos(θ + β) = cos(θ)cos(β) - sin(θ)sin(β), then from Equation (18), the angle β of upward inclination of the vascular robot output by the gravity - buoyancy compensator compared with the ideal pitch angle can be obtained as: From equations (15), (16) and (17), the angular velocity Ω of the three-dimensional rotating magnetic field output by the gravity-buoyancy compensator can be obtained. 1 It is: The sliding mode controller based on gravity - buoyancy compensation described in Step 3 is: Denote the expected motion trajectory of the vascular robot as [x d (t), y d (t), z d (t)] T . Suppose the initial rolling angular velocity of the robot is Then the expected yaw angle ψ and the expected pitch angle θ of the vascular robot are respectively: Assume the roll angular velocity of the vascular robot is consistent with the rotation angular velocity Ω of the three-dimensional rotating magnetic field 1 , then under the action of the gravity-buoyancy compensator, the yaw angle ψ s , pitch angle θ s , and roll angular velocity are as follows: Finally, the expected motion trajectory of the above vascular robot [x d (t), y d (t), z d (t)] T is subjected to dual quaternion transformation with the attitude angle of the vascular robot after gravity-buoyancy compensation to obtain the motion description in the dual quaternion framework Thus, the dual quaternion motion of the expected coordinate system of the vascular robot is used as the input of the control system; The control objective of the vascular robot is: by controlling the pose and trajectory error of the vascular robot's body coordinate system O b X b Y b Z b relative to the desired coordinate system O d X d Y d Z d to make the actual motion state of the vascular robot gradually converge to the motion state under the desired coordinate system ​ Based on the kinematic and dynamic equations (3) and (4) of the posture and trajectory integration of the vascular robot established in Step 1, design the following sliding mode surface: Among them, is the product operation of even numbers, is the controller parameter, and k si > 0, Design the sliding mode control law as: Among them, is the sliding mode variable, sgn(·) is the sign function, and are the controller parameters, and k εi > 0, are the controller parameters, and k ui > 0, is a product operation of dual vectors, is the dual disturbance force; The adaptive sliding mode controller described in Step 4 is: To facilitate the design of an adaptive sliding mode controller, the terms with M in Equation (27) are expressed by the following formula: b ​ Among them, is another expression of the dual mass property inertia matrix M b ; and Π ∈ R 3×1 , through the special property ε 2 = 0 of the dual dipole operation, we can further obtain is a three-dimensional dual vector; due to the particularity of the dual mass property inertia matrix M b , it can be easily obtained that Π. And for the calculation of , it can be deduced from equations (29)-(31): for any x ∈ R 3×1 , y ∈ R 3×1 , we have: where δ(x, y) = δ 1 (x) + δ 2 (y), and the calculation methods of δ 1 (x) and δ 2 (y) are as follows: Due to the complex working environment and variable working content of the vascular robot, during the modeling process of the vascular robot, its mass and inertia often have uncertainty and variability. Therefore, for the dual mass characteristic inertia matrix and the dual disturbance force the estimated value of the design parameter adaptive law is as follows: Among them, is the controller parameter, is the controller parameter, and: Substitute Equation (32) and Equation (33) into the sliding mode control law of Equation (27), then the adaptive sliding mode control law can be obtained as:

Images