CN111904486B

CN111904486B - Adaptive sliding mode tracking control method for spiral vascular robot with integrated posture and track

Info

Publication number: CN111904486B
Application number: CN202010418859.XA
Authority: CN
Inventors: 周淼磊; 张晨; 张颖; 韩志武
Original assignee: Jilin University
Current assignee: Jilin University
Priority date: 2020-05-18
Filing date: 2020-05-18
Publication date: 2025-05-30
Anticipated expiration: 2040-05-18
Also published as: CN111904486A

Abstract

A spiral vessel robot attitude and orbit integrated self-adaptive sliding mode tracking control method belongs to the technical field of control. The invention aims to fully consider the influence of the attitude and orbit coupling and the gravity-buoyancy of a vascular robot, and improves the control precision of the spiral type attitude and orbit integrated self-adaptive sliding mode tracking control method of the vascular robot. The method comprises the steps of establishing a gesture-rail integrated kinematic and dynamic model of the spiral vascular robot, designing a gravity-buoyancy compensator to compensate the sinking action of gravity-buoyancy in the vertical direction, designing a sliding mode controller based on gravity-buoyancy compensation, and designing a self-adaptive sliding mode controller based on the sliding mode controller designed in the step three. The invention can better inhibit the jitter phenomenon of sliding mode control and improve the gesture track tracking control precision. Has great practical value and theoretical significance for the application of the vascular robot in minimally invasive surgery.

Description

Spiral vascular robot attitude and orbit integrated self-adaptive sliding mode tracking control method

Technical Field

The invention belongs to the technical field of control.

Background

In recent years, while technology is vigorously developed, irregular living habits cause more and more people to be bothered by cardiovascular and cerebrovascular diseases. The traditional operation treatment has the defects of large radiation injury, large postoperative wound and slow recovery, and the unique characteristics of the minimally invasive operation can overcome the defects of the traditional operation treatment, so that the traditional operation treatment becomes a focus of attention of the medical community. The combination of minimally invasive surgery and robot technology is a mainstream development trend in the medical community, and has a wide development prospect.

The vascular robot is a new method for trying on the problem of cardiovascular and cerebrovascular diseases, and the vascular robot is a micro-robot which works in a vascular environment and can realize the working requirement through an external controller. In life, the vascular robot can be used for carrying out targeted treatment on cancerous tissues and accurately delivering medicines to the affected parts, and can also be used for regularly removing blood sediments such as cholesterol, fat and the like and preventing cardiovascular and cerebrovascular diseases. Meanwhile, the vascular robot also has a corresponding detection function, and can diagnose and detect various organs and tissues of a human body. A doctor can adopt a reasonable treatment method based on the detection data of the vascular robot, so that the operation is greatly reduced and the operation injury to a patient is relieved finally. In addition, the vascular robot has the functions of removing parasites and bacteria, cleaning wounds, crushing stones and the like, can efficiently solve the disease problem of patients, and is convenient for people to live. The injury caused by the traditional operation can be greatly reduced with the help of the vascular robot, so that the research on the vascular robot gradually evolves into an important research direction in the medical field.

The vascular robot with the spiral structure consists of a magnetic material head part and a spiral tail part, and is powered by an external three-dimensional rotating magnetic field. The dynamic pressure lubrication effect generated by the spiral motion of the robot can effectively prevent the robot from contacting the vessel wall to protect the vessel, and the vessel robot with the spiral structure driven by the external rotating magnetic field has higher academic research value and wide medical application prospect by comprehensively considering the safety and the stability and the realizability of the energy supply mode. The object to which the present invention is directed is therefore a spiral vascular robot. Traditionally, researchers have used modeling the motion trajectories and poses of vascular robots, respectively, but vascular robot motion is actually affected by the coupling of the poses trajectories. According to the working requirements of the vascular robot, the vascular robot must be ensured not to damage the vascular wall of a human body. However, in actual operation, due to the influence of gravity-buoyancy in the vertical direction, the vascular robot tends to sink and deviate relative to the expected motion track. And the control effect of the traditional control method on the vascular robot is poor. In order to avoid the problems, the practical situation is considered when the modeling and the control of the vascular robot are carried out, so that a modeling solution which can fully consider the influence of the coupling of the pose and the orbit of the vascular robot and a control solution which can consider the gravity and the buoyancy compensation are found, and the method has great significance for the application of the vascular robot in the minimally invasive surgery.

Disclosure of Invention

The invention aims to fully consider the influence of the attitude and orbit coupling and the gravity-buoyancy of a vascular robot, and improves the control precision of the spiral type attitude and orbit integrated self-adaptive sliding mode tracking control method of the vascular robot.

The method comprises the following steps:

Simultaneously describing the gesture and the track motion of the spiral vascular robot by adopting dual quaternions, and establishing a gesture-track integrated kinematics and dynamics model of the spiral vascular robot;

step two, designing a gravity-buoyancy compensator to compensate the sinking action of gravity-buoyancy in the vertical direction aiming at the fact that the spiral vascular robot is influenced by gravity-buoyancy when moving in blood, so that the actual movement track is sunk and deviated from the expected track;

Step three, designing a sliding mode controller based on gravity-buoyancy compensation based on the spiral vascular robot model established in the step one and the gravity-buoyancy compensator designed in the step two;

step four, designing self-adaptive sliding mode control based on the sliding mode controller designed in the step three;

The specific process for establishing the gesture-rail integrated kinematics and dynamics model comprises the following steps:

Pose rail error of vascular robot body coordinate system O _bX_bY_bZ_b described by unit dual quaternion relative to expected coordinate system O _dX_dY_dZ_d The method comprises the following steps:

Wherein, AndThe vessel robot expected coordinate system O _dX_dY_dZ_d and the body coordinate system O _bX_bY_bZ_b which are respectively expressed by unit dual quaternions are respectively relative to the pose rail of the inertial coordinate system OXYZ, the 'x' represents the multiplication of the dual quaternions,Representing the conjugation of dual quaternions;

Speed rotation error of vascular robot body coordinate system O _bX_bY_bZ_b relative to desired coordinate system O _dX_dY_dZ_d Expressed under the coordinate system O _bX_bY_bZ_b as:

Wherein, For the representation of the velocity spin of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the inertial coordinate system ozz in the robot body coordinate system O _bX_bY_bZ_b,For the representation of the velocity spin of the vascular robot desired coordinate system O _dX_dY_dZ_d relative to the inertial coordinate system ozz in the desired coordinate system O _dX_dY_dZ_d, (·) ^* represents the conjugation of the dual quaternion;

and can be based on the derivation Attitude and orbit integrated kinematics and dynamics equation:

Wherein M ^b is a dual mass characteristic inertia matrix, The action point in the coordinate system O _bX_bY_bZ_b is the dual force rotation of the center of mass of the vascular robot, the 'X' represents the cross multiplication operation of the dual quaternion,Representing an operation of a matrix and a dual quaternion for the dual quaternionDefinition of the definitionThe operation of (1) isEpsilon represents the dual unit and is definedThe operation of (1) is Is any dual quaternion;

The gravity-buoyancy compensator is specifically designed according to the following steps:

Defining the desired movement speed of the vascular robot as The vascular robot in the blood is mainly subjected to the total driving forceGravity-buoyancy of robotFluid resistanceIs effective in (1);

Total driving force The expression of (2) is:

Wherein, Represents the driving force generated by the spiral tail rotation of the vascular robot under the action of the three-dimensional rotating magnetic field,Is magnetic pulling force generated by magnetic gradient, n is the rotation number of the spiral tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, alpha ₁ is the spiral lead angle of the tail of the vascular robot,The rotation angular velocity of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity,Zeta _⊥ is the axial drag coefficient perpendicular to the helical tail, which is the magnetic field gradient, anZeta ₁₁ is the axial drag coefficient parallel to the helical tail, andΗ is the viscosity coefficient of blood, and κ is the diameter of the spiral tail of the vascular robot;

fluid resistance Fluid resistance generated by blood flow impact on vascular robot head and spiral tailThe composition is expressed as:

Robot gravity-buoyancy Is gravity G and buoyancyThe resultant force acting in the vertical direction is expressed as:

Wherein ρ is the density of the vascular robot, ρ _f is the density of the blood, Gravitational acceleration;

Counteracting the effect of gravity-buoyancy on vascular robot, fluid resistance is required And total driving forceIs to be combined with the action of (a), namely:

Wherein the method comprises the steps of For designing the gravity-buoyancy compensator, a posture coordinate system p of the vascular robot is established, an origin O _p of the coordinate system is positioned at the gravity center of the vascular robot, and a coordinate system x _p axis and the vascular robot are actually pointedCoincident, the z _p axis is perpendicular to the actual direction of the vascular robotFor the vascular robot to counteract the motion speed of gravity-buoyancy effect,

Obtained according to formula (8):

Desired movement speed of vascular robot Decomposition into horizontal componentsAnd vertical componentIt can be known that the vascular robot expects the motion speedHorizontal component of (2)And (3) withHorizontal component of (2)Equal, i.e. haveRotational angular velocity of vascular robotThe rotation angular velocity omega ₁ of the three-dimensional rotation magnetic field has the following relation:

from the above analysis it is possible to obtain:

Wherein, Is a unit vector;

When (when) When available according to formulas (9) and (10):

When (when) When it willSplit into two forces, i.e. components parallel to the x _p axisWith a component perpendicular to the x _p axisF _⊥p atThe direction has no component, and because of uncertainty and variability in the direction of the magnetic field gradient, it is assumed thatActing on the x _pz_p plane and in the direction of the desired speed of movement of the vascular robotSymmetrical about the x _p axis, then similar toIs characterized by that in the stress analysis of (a),Can be decomposed into components parallel to the x _p axisWith a component perpendicular to the x _p axisF _{⊥p_1} atThe directions have no component, then there are:

Wherein, AndRespectively representThe number of projections in the z _p axis and the x _p axis, andThe number of projections at the z _p axis and the x _p axis,The unit vectors representing the z _p axis direction and the x _p axis direction, respectively, are obtained by substituting the formulas (10) and (13) into the formula (9):

Due to Defining a desired pitch angle of a vascular robot asSpeed of the speedProjection into the x _p axis, z _p axis direction, and is obtainable according to equation (14):

The mechanical geometrical relationship of the attitude coordinate system p of the vascular robot is obtained by analyzing:

Wherein beta is the angle of the vessel robot at which the expected pitch angle is inclined upwards;

similar to that in formula (16) Is used for the analysis of (a),The following relationship is also available:

according to formula (15), formula (16) and formula (17), there is obtained:

According to cos (θ+β) =cos (θ) cos (β) -sin (θ) sin (β), the angle β of the upward inclination of the vascular robot output by the gravity-buoyancy compensator compared with the ideal pitch angle can be obtained by the formula (18) is:

From the equation (15), the rotational angular velocity Ω ₁ of the three-dimensional rotating magnetic field output by the gravity-buoyancy compensator is as follows:

the sliding mode controller based on gravity-buoyancy compensation in the third step is as follows:

the expected motion trail of the vessel recording robot is [ x _d(t),y_d(t),z_d(t)]^T, the initial rolling angle speed of the robot is set The desired yaw angle ψ and the desired pitch angle θ of the vascular robot are respectively:

assuming the roll angle speed of the vascular robot The yaw angle psi ^s, the pitch angle theta ^s and the roll angle speed of the vascular robot are consistent with the rotation angle speed omega ₁ of the three-dimensional rotating magnetic field under the action of the gravity-buoyancy compensatorThe method comprises the following steps:

Finally, the expected motion trail [ 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 are carried out Performing dual quaternion transformation to obtain motion description under dual quaternion frameThereby moving the dual quaternion of the expected coordinate system of the vascular robotAs input to the control system;

the control target of the vascular robot is that the attitude and orbit error of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the expected coordinate system O _dX_dY_dZ_d is controlled Making the actual motion state of the vascular robotProgressive convergence to a state of motion in a desired coordinate system

Based on the vessel robot pose-rail integrated kinematics and dynamics equation (3) and the formula (4) established in the step one, the following sliding mode surface is designed:

Wherein, In order to operate on the product of an even number,In order to control the parameters of the controller,And k _si is >0, and,The design sliding mode control law is as follows:

Wherein, As a variable of the sliding mode, the sliding mode is changed,Sgn (·) is a sign function, and In order to control the parameters of the controller,And k _εi is >0, and, In order to control the parameters of the controller,And k _ui is >0, and, For a product operation of the dual vectors,Is a dual interference force;

the self-adaptive sliding mode controller in the fourth step is as follows:

To facilitate the design of an adaptive slip-mode controller, the term with M ^b in equation (27) is represented by:

Wherein, Another expression of the dual mass characteristic inertia matrix M ^b; And pi epsilon R _3×1, Further deriving from the special property epsilon ² =0 of the dipole operationIs a three-dimensional dual vector, pi is easy to obtain due to the specificity of the dual mass characteristic inertia matrix M ^b, and is related toFrom equations (29) - (31), for any x ε R _3×1,y∈R_3×1, there is:

wherein δ (x, y) =δ ₁(x)+δ₂ (y), and δ ₁(x),δ₂ (y) is calculated as follows:

Because of the complex working environment and variable working contents of the vascular robot, the mass and inertia of the vascular robot often have uncertainty and variability in the modeling process of the vascular robot, so the inertial matrix is a dual mass characteristic inertial matrix And dual interference forceEstimate of (2)The design parameter adaptation law is as follows:

Wherein, In order to control the parameters of the controller,In order to control the parameters of the controller,And:

Taking equations (32) and (33) into the slip-form control law of equation (27), an adaptive slip-form control law can be obtained as:

Aiming at the modeling and tracking control problems of the vascular robot, the invention establishes a vascular robot gesture-rail integrated dynamics and kinematics model based on dual quaternion theory, and solves the problem that the use of the vascular robot is affected due to the mutual coupling of gesture tracks of the vascular robot in actual operation. The gravity-buoyancy compensator is designed to counteract the sinking action in consideration of the phenomenon that the vascular robot has sinking and offset motion under the action of gravity-buoyancy in the vertical direction. An adaptive slip-form control with gravity-buoyancy compensation is designed based on the built model and the constructed gravity-buoyancy compensator. Finally, simulation analysis proves that compared with a sliding mode controller, the designed self-adaptive sliding mode controller has better rapidity, can better inhibit the shaking phenomenon of sliding mode control, and improves the gesture track tracking control precision. Has great practical value and theoretical significance for the application of the vascular robot in minimally invasive surgery.

Drawings

FIG. 1 is a diagram of a coordinate system transformation of a dual quaternion representation;

FIG. 2 is a schematic illustration of a vascular robot with a sink bias due to gravity-buoyancy effects and with gravity-buoyancy compensation;

FIG. 3 is a schematic view of the physical structure and parameters of the vascular robot;

FIG. 4 is a force analysis diagram of gravity-buoyancy compensation of a vascular robot;

FIG. 5 is a block diagram of a slipform controller of the vascular robot;

FIG. 6 is a motion profile tracking graph of a vascular robot tracking a linear reference profile;

FIG. 7 is a graph of motion trajectory error for a vascular robot tracking a linear reference trajectory;

FIG. 8 is a yaw angle tracking graph of a vascular robot tracking a linear reference trajectory;

FIG. 9 is a graph of yaw angle tracking error of a vascular robot tracking a linear reference trajectory;

FIG. 10 is a graph of pitch tracking of a vascular robot tracking a linear reference trajectory;

FIG. 11 is a graph of pitch tracking error of a vascular robot tracking a linear reference trajectory;

FIG. 12 is a graph of the roll angle velocity tracking of a vascular robot tracking a linear reference trajectory;

FIG. 13 is a graph of roll angle velocity tracking error of a vascular robot tracking a linear reference trajectory;

FIG. 14 is a motion profile tracking graph of a vascular robot tracking a curvilinear reference profile;

FIG. 15 is a graph of motion trajectory error for a vascular robot tracking a curvilinear reference trajectory;

FIG. 16 is a yaw angle tracking graph of the vascular robot tracking a curvilinear reference trajectory;

FIG. 17 is a graph of yaw angle tracking error for a vascular robot tracking a curvilinear reference trajectory;

FIG. 18 is a graph of pitch tracking of a vascular robot tracking a curvilinear reference trajectory;

FIG. 19 is a graph of pitch tracking error of a vascular robot tracking curve type reference trajectory;

FIG. 20 is a roll angle velocity tracking graph of a vascular robot tracking a curvilinear reference trajectory;

Fig. 21 is a graph of the roll angle velocity tracking error of the vascular robot tracking the curved-type reference trajectory.

Detailed Description

The invention comprises the following steps:

establishing a posture-rail integrated dynamics and kinematics model of the vascular robot by utilizing dual quaternions;

step two, because the vascular robot can sink and deviate under the influence of gravity-buoyancy in the actual motion, a gravity-buoyancy compensator is designed for compensation;

Step three, designing a sliding mode controller based on gravity-buoyancy compensation based on the vascular robot model established in the step one and the gravity-buoyancy compensator designed in the step two;

And step four, designing a self-adaptive sliding mode controller based on the sliding mode controller designed in the step three.

The specific process of the first step is as follows:

Wherein, AndThe desired coordinate system O _dX_dY_dZ_d and the body coordinate system O _bX_bY_bZ_b of the vascular robot, which are expressed by unit dual quaternions, respectively, are relative to the pose rails of the inertial coordinate system ozz, respectively. "x" means multiplication of dual quaternions,Representing the conjugation of the dual quaternion.

Wherein, Is a representation of the velocity spin of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the inertial coordinate system ozz in the robot body coordinate system O _bX_bY_bZ_b.Is a representation of the velocity rotation of the vascular robot desired coordinate system O _dX_dY_dZ_d relative to the inertial coordinate system ozz in the desired coordinate system O _dX_dY_dZ_d. (. Cndot.) ^* represents the conjugation of the dual quaternion.

And can be based on the derivationAttitude and orbit integrated kinematics and dynamics equation:

wherein M ^b is a dual mass characteristic inertia matrix. The dual force rotation of the acting point in the coordinate system O _bX_bY_bZ_b as the center of mass of the vascular robot is represented. "x" represents a cross multiplication operation of dual quaternions,Representing an operation of the matrix and the dual quaternion. For dual quaternionsDefinition of the definitionThe operation of (1) isEpsilon represents the dual unit and is definedThe operation of (1) is Is any dual quaternion.

The specific process of the second step is as follows:

Defining the desired movement speed of the vascular robot as The vascular robot in the blood is mainly subjected to the total driving forceGravity-buoyancy of robotFluid resistanceIs effective in (1). Under the action of gravity and buoyancy in the vertical direction, the vascular robot tends to sink and deviate relative to the expected movement speed, so that the gravity-buoyancy compensator is designed to enable the vascular robot to achieve the expected movement effect. The input of the compensator is the expected motion speed of the vascular robot, and the output is the upward inclination angle of the robot compared with the ideal pitch angle and the rotation angular speed of the three-dimensional rotating magnetic field.

Total driving forceThe expression of (2) is:

Wherein, Represents the driving force generated by the spiral tail rotation of the vascular robot under the action of the three-dimensional rotating magnetic field,Is a magnetic pull force generated by a magnetic gradient. n is the number of rotations of the tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, alpha ₁ is the tail helix angle of the vascular robot,The rotation angular velocity of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity,Is a magnetic field gradient. Zeta _⊥ is the axial drag coefficient perpendicular to the helical tail, andZeta ₁₁ is the axial drag coefficient parallel to the helical tail, andΗ is the blood viscosity coefficient and κ is the diameter of the spiral tail of the vascular robot.

Fluid resistanceFluid resistance generated by blood flow impact on vascular robot head and spiral tailThe composition can be expressed as:

Wherein ρ is the density of the vascular robot, ρ _f is the density of the blood, Gravitational acceleration.

From the stress analysis of the vascular robot, if the action effect (sinking) of gravity-buoyancy on the vascular robot is to be offset, the fluid resistance is requiredAnd total driving forceIs to be combined with the action of (a), namely:

Wherein,

For designing the gravity-buoyancy compensator, a posture coordinate system p of the vascular robot is established. The origin O _p of the coordinate system is positioned at the gravity center of the vascular robot, and the x _p axis of the coordinate system and the vascular robot are actually pointed toCoincident, the z _p axis is perpendicular to the actual direction of the vascular robot The motion speed of the vascular robot is used for counteracting the gravity-buoyancy effect.

Obtainable according to formula (8):

Desired movement speed of vascular robot Decomposition into horizontal componentsAnd vertical componentIt can be known that the vascular robot expects the motion speedHorizontal component of (2)And (3) withHorizontal component of (2)Equal, i.e. have

Rotational angular velocity of vascular robotThe rotation angular velocity omega ₁ of the three-dimensional rotation magnetic field has the following relation:

from the above analysis it is possible to obtain:

Wherein, Is a unit vector.

When (when)When available according to formulas (9) and (10):

When (when) When it willSplit into two forces, i.e. components parallel to the x _p axisWith a component perpendicular to the x _p axis(F _⊥p inThe direction has no component). Again due to uncertainty and variability in the direction of the magnetic field gradient, it is assumed thatActing on the x _pz_p plane and in the direction of the desired speed of movement of the vascular robotSymmetrical about the x _p axis. Then is similar toIs characterized by that in the stress analysis of (a),Can be decomposed into components parallel to the x _p axisWith a component perpendicular to the x _p axis(F _{⊥p_1} inNo component of direction), then there is:

Wherein, AndRespectively representThe number of projections in the z _p axis and the x _p axis, andA number of projections at the z _p axis and the x _p axis.The unit vectors respectively indicate the z _p axis direction and the x _p axis direction.

Substituting the formulas (10) and (13) into the formula (9) yields:

where β is the angle at which the vascular robot expects the pitch angle to tilt upwards.

Similar to that in formula (16)Is used for the analysis of (a),The following relationship is also available:

from the formula (15), the formulas (16) and (17), it is possible to obtain:

the specific process of the third step is as follows:

Finally, the expected motion trail [ 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 are carried out Performing dual quaternion transformation to obtain motion description under dual quaternion frameThereby moving the dual quaternion of the expected coordinate system of the vascular robotAs input to the control system.

The control target of the vascular robot is that the attitude and orbit error of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the expected coordinate system O _dX_dY_dZ_d is controlledMaking the actual motion state of the vascular robotProgressive convergence to a state of motion in a desired coordinate system

Based on the vessel robot pose-rail integrated kinematics and dynamics equation (3) and the formula (4) established in the step 1, the following sliding mode surface is designed:

Wherein, Is a product operation on an even number.In order to control the parameters of the controller,And k _si is >0, and,

The design sliding mode control law is as follows:

Wherein, As a variable of the sliding mode, the sliding mode is changed,Sgn (·) is a sign function, and In order to control the parameters of the controller,And k _εi is >0, and, In order to control the parameters of the controller,And k _ui is >0, and, Is a product operation of dual vectors.Is a dual interference force.

The specific process of the fourth step is as follows:

the sliding mode controller designed in the third step has a jitter phenomenon, and the vascular robot often needs to face different working tasks and working environments in the clinical medical application process, so that model parameters of the vascular robot have variation characteristics and uncertainty which cannot be estimated, (mainly reflected in inaccuracy and dual interference force of a dual mass characteristic inertia matrix M ^b in the vascular robot) To solve the problems of uncertainty and inaccuracy of model parameters and to suppress jitter in the sliding mode controller), an adaptive sliding mode controller is designed to improve the control performance and control accuracy of the vascular robotic system.

Wherein, In order to control the parameters of the controller, In order to control the parameters of the controller,And:

The present invention will be described in further detail with reference to the accompanying drawings

First, the quaternion and dual quaternion are basically described

Quaternion

Quaternion is a mathematical theory proposed by Hamilton in 1843, which takes the form:

Where l ₀ is a real number, representing the scalar portion of the quaternion; And represents the vector part of the quaternion, l _v1、l_v2、l_v3 is a real number, Are all orthogonal vectors, and

The basic algorithm of quaternion is:

Wherein l ₁、l₂ represents a quaternion, λ is a real number, l ^* represents the conjugation of the quaternion l, l represents the modulus of the quaternion l, the quaternion defining the modulus as 1 is a unit quaternion, l ^-1 represents the inverse of the quaternion l, vec (l) represents the operation of vector conversion on the quaternion l, i.e. the first element of l is taken as 0, so that l becomes a vector.

For easy calculation, matrix and quaternion are combinedThe operation is defined as:

Wherein, Is a fourth order matrix.

"X" denotes multiplication of quaternions, specifically calculated as:

Wherein, E ₃ represents a third-order unit array, anIs a cross-over matrix.

By derivation:

In particular, in AndThe following calculations are defined, respectively:

The "x" operation defining the quaternion is:

To even number

The even number is proposed by the math Clifford and defined as:

Wherein, a, E represents a dual unit, e ² = 0 and e +.0.

The following operations are performed on the even numbers:

Wherein, Are both dual numbers, and μ is a real number.

For ease of calculation, define pairs of even numbersThe operation is as follows:

Dual vector

When the even number real number part and the dual number part are both three-dimensional vectors, the dual number is defined as a dual vector, i.eWherein the method comprises the steps ofWhen the real part of the dual vector is a free vector and the dual part is a positioning vector (i.e., the vector of the real part is independent of the selected reference point and the vector of the dual part is related to the selected reference point), this dual vector is called a rotation. The basic operation of the dual vector is as follows:

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

from the expression of equation (1012), it is known that "< >" represents a dual vector in which each element is positive.

Definition of the definitionA product operation of dual vectors, which is specifically calculated as:

and then can be obtained by calculation:

As can be seen from formula (1014), through The result of the operation is a new dual vector.

An inner product operation of the dual vectors is defined, the symbol is "|| -", and the specific calculation is as follows:

as can be seen from the above equation, the result of the inner product operation of the dual vector is a real number.

Dual momentum

Definition of the coupling momentum of the rigid body when the rigid body centroid E _c is the reference point of the coupling momentumCan be expressed as:

Wherein m is the mass of the rigid body, Is the velocity of the rigid body and,Representing the moment of inertia of the rigid body line with the centroid E _c as a reference point, Λ _E representing the moment of inertia matrix of the centroid E _c in the body coordinate system,Is the angular velocity of the rigid body,Representing the rigid body angular momentum with centroid E _c as the reference point.

Deriving equation (1016), then obtaining the Newton-Euler rigid body dynamics equation for the reference point E _c at the centroid:

Wherein, In order to couple the rotation of the force,And (3) withExternal force and moment on the centroid action point are respectively.

Dual quaternion

The dual quaternion is a quaternion with each element being an even number or an even number with the quaternion as a real part and a dual part, and is defined as:

wherein, l and Represents any quaternion; The representation is made for an even number, Representing the dual vector;

as can be seen from the definition of dual quaternions, the basic algorithm of dual quaternions and quaternions is similar and will not be described here again.

For ease of later analysis understanding, the matrix and dual quaternion are hereThe operation is defined as follows:

Wherein, Is an eighth order matrix.

For ease of later analysis understanding, operations similar to those defined by quaternions (1006) and (1007) are similarly defined in dual quaternionsAndIs performed by the computer system.

Establishing a gesture-rail integrated dynamics and kinematics model of the vascular robot by using dual quaternions:

The vascular robot body coordinate system is defined as O _bX_bY_bZ_b, the inertial coordinate system is defined as OXYZ, and the expected coordinate system is defined as O _dX_dY_dZ_d.

The vascular robot gesture kinematics equation based on the unit quaternion firstly describes the three-dimensional rotation motion of the vascular robot by using the unit quaternion.

The Euler's theorem indicates that when a rigid body rotates around a fixed point, it is understood that the rigid body rotates around a certain linear axis passing through the fixed point by a certain angle, and the coordinate system n is represented by a quaternary numberRotation of the angle α yields a motion description of the coordinate system m:

Wherein, So l _mn is the unit quaternion. And is also provided with

Since the inertial coordinate system ozz is a reference fixed coordinate system in the motion process of the vascular robot, the attitude kinematic equation of the vascular robot body coordinate system O _bX_bY_bZ_b expressed by using the unit quaternion relative to the inertial coordinate system ozz is as follows:

Wherein, The angular velocity of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the inertial coordinate system ozz is represented in the robot body coordinate system O _bX_bY_bZ_b and the inertial coordinate system ozz, respectively.

Similarly, the attitude kinematic equation of the desired coordinate system O _dX_dY_dZ_d relative to the inertial coordinate system ozz can be expressed in terms of unit quaternions:

Wherein, The angular velocity of the vascular robot desired coordinate system O _dX_dY_dZ_d relative to the inertial coordinate system ozz is represented in the desired coordinate system O _dX_dY_dZ_d and the inertial coordinate system ozz, respectively.

Since the unit quaternion has multiplicative error property, the attitude error l _e of the vascular robot body coordinate system O _bX_bY_bZ_b with respect to the desired coordinate system O _dX_dY_dZ_d is:

Angular velocity error of vascular robot body coordinate system O _bX_bY_bZ_b with respect to desired coordinate system O _dX_dY_dZ_d Expressed under the coordinate system O _bX_bY_bZ_b as:

Wherein,

The equation (1) is subjected to differential operation and substituted into equations (1021), (1022) and (1024), and the attitude error kinematic equation of the vascular robot body coordinate system O _bX_bY_bZ_b with respect to the desired coordinate system O _dX_dY_dZ_d is:

Wherein, Is a representation of the angular velocity of the error of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the desired coordinate system O _dX_dY_dZ_d under the coordinate system O _dX_dY_dZ_d.

The vessel robot pose rail integrated kinematic equation based on the dual quaternion is that the unit quaternion only describes three-dimensional rotation motion (namely pose motion) of the vessel robot, and the dual quaternion can express rotation and translation motion in the same mathematical expression, so that the dual quaternion is adopted to describe the three-dimensional rotation and translation motion (namely pose rail integrated motion) of the vessel robot.

From Chasles theorem, spiral motion can be used to describe the general motion of a rigid body, while spatial six-degree-of-freedom rotational motion and translational motion of a rigid body coordinate system can be simultaneously described using dual quaternions. FIG. 1 is a diagram depicting a W coordinate system first about an axis of rotationRotating alpha angle re-edgeThe direction of the line translates d "distance to the K coordinate system. This process is expressed in unit dual quaternion as:

Wherein,

A straight line is arranged in the space and is respectively expressed as a W coordinate system and a K coordinate systemAnd (3) withObtainable according to formula (6)The method further comprises the following steps:

Wherein, Is a representation of the position of the K coordinate system relative to the W coordinate system in the K coordinate system. The motion of the K coordinate system relative to the W coordinate system can be described as:

Wherein, Is a representation of the position of the K coordinate system relative to the W coordinate system in the W coordinate system.

Combining the formula (1025) and deriving the formula (1028) to obtain the attitude and orbit integrated kinematic equation described by the dual quaternion theory:

Wherein, Is a representation of the velocity rotation of the K-coordinate system relative to the W-coordinate system in the K-coordinate system,Is a representation of the velocity rotation of the K-coordinate system relative to the W-coordinate system in the W-coordinate system.

The kinematic equation of the body coordinate system O _bX_bY_bZ_b of the vascular robot based on the unit dual quaternion relative to the inertial coordinate system OXYZ, namely the attitude and orbit integrated kinematic equation of the vascular robot under the inertial coordinate system, can be obtained by combining the formula (1021) and the formula (1029), and is as follows:

Wherein, The speed rotation of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the inertial coordinate system ozz is represented in the robot body coordinate system O _bX_bY_bZ_b and the inertial coordinate system ozz, respectively.

Also available in combination with equation (1022) and equation (1029), the pose-orbit integrated kinematic equation describing the inertial coordinate system ozz of the vascular robot relative to the desired coordinate system O _dX_dY_dZ_d in the dual quaternion theory is:

Wherein, The desired coordinate system O _dX_dY_dZ_d of the vascular robot is represented in the desired coordinate system O _dX_dY_dZ_d and the inertial coordinate system ozz by the speed rotation of the vascular robot relative to the inertial coordinate system ozz, respectively.

The unit dual quaternion has similar multiplicative error property as the unit quaternion, so that the pose rail error of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the expected coordinate system O _dX_dY_dZ_d The method comprises the following steps:

The pose-orbit integrated kinematic equation of the vascular robot body coordinate system O _bX_bY_bZ_b under the dual quaternion frame relative to the expected coordinate system O _dX_dY_dZ_d can be obtained according to the formula (1025) and the formula (1029):

Wherein, AndIs a representation of the speed rotation error of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the desired coordinate system O _dX_dY_dZ_d under the coordinate system O _bX_bY_bZ_b and the coordinate system O _dX_dY_dZ_d.

Vessel robot attitude and orbit integrated dynamics equation based on dual quaternion:

For ease of analysis, assuming that the vessel robot is a rigid body, the vessel robot dynamics equation with the centroid as the reference point can be given according to equation (1017):

Wherein, The action point in the coordinate system O _bX_bY_bZ_b is indicated as the dual force rotation of the centroid of the vascular robot, Representing the resultant force of the point of action on the vascular robot mass under the coordinate system O _bX_bY_bZ_b, whereinIndicating the total driving force and,Representing the gravity-buoyancy of the robot,The fluid resistance is indicated by the expression,Representing dual interference forces.

Representing the resultant moment acting on the vascular robot mass in the coordinate system O _bX_bY_bZ_b, whereinThe driving moment, the gravity-floating moment, the fluid resistance moment and the dual disturbance moment of the robot are respectively represented.Is the dual momentum taking the center of mass of the vascular robot as a reference point.

Defining a dual mass characteristic inertia matrix as:

Wherein Λ _b is a vascular robot moment of inertia matrix under the body coordinate system O _bX_bY_bZ_b, and m is the vascular robot mass.

Definition of the definitionCombining equation (1016) with equation (1019), there are:

deriving equation (1034) from equation (1036) yields:

based on the angular velocity error represented by the quaternion in equation (1024), the velocity rotation error of the vascular robot body coordinate system O _bX_bY_bZ_b relative to the vascular robot desired coordinate system O _dX_dY_dZ_d in the coordinate system O _bX_bY_bZ_b represented by the dual quaternion is:

deriving the equation (1038), and deriving from the equations (1027) and (1033):

Substituting the formula (1037) into the formula (1039), and further deriving to obtain an attitude and orbit integrated dynamics equation of the vascular robot body coordinate system O _bX_bY_bZ_b based on the dual quaternion theory relative to the expected coordinate system O _dX_dY_dZ_d, wherein the attitude and orbit integrated dynamics equation is as follows:

in summary, the vessel robot pose-rail integrated kinematics and dynamics equation established based on dual quaternion is as follows:

Designing a gravity-buoyancy compensator of the vascular robot:

When the vascular robot moves in blood, in order to meet the working requirement that the vascular robot can reciprocate in blood vessels, the vascular robot is assumed to reverse the blood flow velocity Movement, i.e. desired movement speed of vascular robotWith blood flow rateIn the opposite direction of the light,Is the actual movement speed of the vascular robot. The vascular robot in blood is mainly subjected to the following forces, namely the total driving forceGravity-buoyancy of robotFluid resistance(Since the vascular robot is subjected to very small electrostatic and van der Waals forces, this is omitted). Under the action of gravity-buoyancy in the vertical direction, the vascular robot tends to sink and deviate relative to the expected movement speed, so that the gravity-buoyancy compensator is designed, the input is the expected movement speed of the vascular robot, and the output is the angle of upward inclination of the robot compared with an ideal pitch angle and the rotation angular speed of a three-dimensional rotating magnetic field. On the basis of a traditional control strategy, the vascular robot is enabled to be matched with the rotation angular velocity of the magnetic field to be inclined and compensated for a certain angle relative to the traditional expected gesture under the different expected motion track requirements, and the vascular robot is enabled to achieve the expected motion effect. A schematic representation of the robot dip offset and the gravity-buoyancy compensation is shown in fig. 2.

Total driving forceThe expression of (2) is:

Wherein, Represents the driving force generated by the spiral tail rotation of the vascular robot under the action of the three-dimensional rotating magnetic field,Is a magnetic pull force generated by a magnetic gradient. n is the number of rotations of the spiral tail of the vascular robot, d is the diameter of the magnetic spherical head of the vascular robot, alpha ₁ is the spiral lift angle of the tail of the vascular robot, ζ _⊥ is the axial resistance coefficient perpendicular to the spiral tail, and ζ ₁₁ is the axial resistance coefficient parallel to the spiral tail.The rotation angular velocity of the vascular robot, V is the volume of the vascular robot, M is the magnetization intensity,Is a magnetic field gradient.

Total driving forceThe calculation of (1) requires the values of two parameters ζ _⊥ and ζ ₁₁, respectively:

Wherein eta is the viscosity coefficient of blood, and kappa is the diameter of the spiral tail of the vascular robot. The physical structure and parameter diagram of the vascular robot are shown in fig. 3.

From the stress analysis of the vascular robot of fig. 2, it is known that if the effect (sinking) of gravity-buoyancy on the vascular robot is to be offset, the fluid resistance is requiredAnd total driving forceIs to be combined with the action of (a), namely:

Wherein,

To design the gravity-buoyancy compensator, a pose coordinate system p of the vascular robot is established as shown in fig. 4. The origin O _p of the coordinate system is positioned at the gravity center of the vascular robot, and the x _p axis of the coordinate system and the vascular robot are actually pointed toCoincident, the z _p axis is perpendicular to the actual direction of the vascular robot A desired movement speed for the vascular robot,The motion speed of the vascular robot is used for counteracting the gravity-buoyancy effect.

Obtainable according to formula (1048):

as can be seen from fig. 4, the vascular robot is expected to move at a desired speed Decomposition into horizontal componentsAnd vertical componentEasily known vascular robot desired movement speedHorizontal component of (2)And (3) withHorizontal component of (2)Equal, i.e. have

It can be derived from fig. 4:

Wherein, Is a unit vector.

When (when)When, according to fig. 4 and equation (49), equation (50) can be obtained:

When (when) When it willSplit into two forces, i.e. components parallel to the x _p axisWith a component perpendicular to the x _p axisEasily known f _⊥p inThe directions have no component, then there are:

Wherein, Respectively representA number of projections at the z _p axis and the x _p axis; The unit vectors respectively indicate the z _p axis direction and the x _p axis direction.

Due to uncertainty and variability in the direction of the magnetic field gradient, it is assumed thatActing on the x _pz_p plane and in the direction of the desired speed of movement of the vascular robotSymmetrical about the x _p axis, then similar toIs characterized by that in the stress analysis of (a),Can be decomposed into components parallel to the axis x _p With a component perpendicular to the x _p axisEasily known f _{⊥p_1} inThe directions have no component, then there are:

Wherein, Respectively representA number of projections at the z _p axis and the x _p axis.

Bringing formula (1050), formula (1053) and formula (1054) to formula (1049) yields:

Due to Definition of the desired pitch angle of the vascular robot in fig. 4 asSpeed of the speedProjection to the x _p axis, z _p axis direction, and is obtained according to equation (55):

By analyzing the mechanical geometry of fig. 4, it is possible to obtain:

Wherein beta is the angle of inclination of the vessel robot at which the pitch angle is desired.

Similar to those in FIG. 4 and equation (1057)Is used for the analysis of (a),The following relationship is also available:

From formula (1056), formula (1057), formula (1058), one can obtain:

Since cos (θ+β) =cos (θ) cos (β) -sin (θ) sin (β), the angle β at which the gravity-buoyancy compensator output of the vascular robot is tilted up compared to the ideal pitch angle is obtained by equation (1059):

By the formula (1056), the formula (1057) and the formula (1058), the rotation angular velocity omega ₁ of the three-dimensional rotation magnetic field output by the gravity-buoyancy compensator is:

designing a sliding mode controller of the vascular robot based on gravity-buoyancy compensation:

A sliding mode control block diagram of the vascular robot is shown in fig. 5.

The expected motion trail of the vessel recording robot is [ x _d(t),y_d(t),z_d(t)]^T, the initial rolling angle speed of the robot is setThe desired yaw angle ψ and the desired pitch angle θ of the vascular robot are respectively:

Based on the vessel robot pose-rail integrated kinematics and dynamics equation (1041) and the formula (1042) established in the step 1, the following sliding die surface is designed:

Wherein, The operation of (1) is shown in the formula (1011).In order to control the parameters of the controller,And k _si is >0, and,

The design sliding mode control law is as follows:

Wherein, As a variable of the sliding mode, the sliding mode is changed,Sgn (·) is a sign function, and In order to control the parameters of the controller,And k _εi is >0, and,In order to control the parameters of the controller,And k _ui is >0, and,And is related toThe operation of (3) is shown in formula (1013).

Slip form controller stability demonstration

For the nonlinear system of the vascular robot, the stability of the sliding mode controller is mainly proved to have two steps, namely firstly proving that the control system can be converged in any initial stateFurther prove that the closed-loop control system is arranged on the sliding mode surfaceStability on the other hand.

The following is a specific proof procedure:

the first step is to prove that the control system can converge to any initial state The Lyapunov function was designed as follows:

wherein, "||·|| calculation of' see formula (1015).

Since M ^b is a positive definite symmetric array, V ₁ is not less than 0, and if and only ifWhen V ₁ =0, deriving the above formula and bringing formula (67) in, it is possible to obtain:

Wherein,

Substituting the vessel robot pose-rail integrated dynamics equation (1042) into the equation (1070) can obtain:

substitution of formula (1068) into formula (1071) yields:

wherein, the specific calculation of </SUB > is shown in formula (1012).

Secondly, proving that the closed-loop control system is on the sliding mode surfaceStability on the other hand.

The following Lyapunov function is defined:

it is easy to know that V ₂ is more than or equal to 0 only V ₂ =0, and deriving the equation (1073):

Further, the method comprises the following steps:

through the process of the formula (1075) This term is derived to yield:

continuing to derive (1076) results in:

Similarly, for the other two items in formula (1075), The derivation process is similar to formulas (1076) and (1077), and is not described here again.

From analysis of the derivation process of equation (1076), equation (1077), equation (1075) can be written as:

When (when) When it is availableSubstitution formula (1078), can be obtained:

Thus, the evidence is obtained.

Step 4, designing an adaptive sliding mode controller of the vascular robot:

the sliding mode controller designed in the step 3 has a jitter phenomenon, and the vascular robot often needs to face different working tasks and working environments in the clinical medical application process, so that model parameters of the vascular robot have variation characteristics and uncertainty which cannot be estimated, (mainly reflected in inaccuracy and dual interference force of a dual mass characteristic inertia matrix M ^b in the vascular robot) To solve the problems of uncertainty and inaccuracy of model parameters and to suppress jitter in the sliding mode controller), an adaptive sliding mode controller is designed to improve the control performance and control accuracy of the vascular robotic system.

To facilitate the design of an adaptive slip-mode controller, the term with M ^b in equation (68) is represented by:

Wherein, Another expression of the dual mass characteristic inertia matrix M ^b; And pi epsilon R _3×1, Further deriving from the special property epsilon ² =0 of the dipole operationIs a three-dimensional dual vector, pi is easy to obtain due to the specificity of the dual mass characteristic inertia matrix M ^b, and is related toFrom formulas (1081) - (1083), for any x ε R _3×1,y∈R_3×1, there is:

taking equations (1084) and (1085) into the slip-form control law of equation (1068), an adaptive slip-form control law is obtained:

Adaptive slip-form controller stability demonstration

Since step 3 has proven that the sliding mode controller of the vascular robot is inThe stability is improved, so that the control system can be converged on the sliding mode surface under the action of the self-adaptive law only needs to be proved in the step. The following Lyapunov function is defined:

Wherein, And hasTo estimate the error, and defineSo to any pairWith V _a being greater than or equal to 0, and if and only if There is V _a =0.

The derivative with respect to time t is obtained for the Lyapunov function described above, and there are:

From equation (70) in step 3, equation (71) can be seen as:

the vessel robot pose-rail integrated dynamics equation (1042) can be obtained by the following formula (1090):

further, by substituting the adaptive sliding mode control law (1087) into the expression (1091), it is possible to obtain:

Bringing the above formula into formula (1089), it is possible to obtain:

The mass coefficients of the pipe robot and the disturbance change are slow, and the mass coefficients are as follows: And then can obtain:

of the formula (1093) This term is derived as follows:

combining formula (1086), formula (1087) and formula (1096), substitution formula 1093) may yield:

Thus, the evidence is obtained.

The following specific examples are employed to demonstrate the beneficial effects of the present invention.

In the first embodiment, the self-adaptive sliding mode controller designed in the step 4 is used for tracking the linear motion trail of the vascular robot, and the control effect of the self-adaptive sliding mode controller is verified by comparing the linear motion trail with the sliding mode controller designed in the step 3.

Vascular robot simulation parameter setting:

Table 1 vascular robot simulation parameters

Selecting a vascular robot starting point as a coordinate origin, and designing the following linear type reference track curve:

the gravity-buoyancy compensator is utilized to carry out posture compensation, the self-adaptive sliding mode control method is adopted to carry out simulation, and the controller parameters are as follows:

Simulation results of the vascular robot tracking the linear motion trail under the action of the sliding mode controller and the self-adaptive sliding mode controller are obtained through simulation, and are shown in fig. 6-13. From fig. 6 and fig. 7, it can be seen that, under the action of the sliding mode control and the adaptive sliding mode control, the vascular robot track control has better tracking precision, and the adaptive sliding mode control is obviously superior to the sliding mode control, and mainly shows that the adaptive sliding mode control can meet the higher control precision requirement, and compared with the sliding mode control, the adaptive sliding mode control saves most of the error convergence adjustment time in the vascular robot track control, and has higher system response rapidity. Fig. 8 to 13 are results of the attitude control of the vascular robot, and it is understood from the figures that the adaptive sliding mode control has higher accuracy of the attitude tracking control and a short adjustment time, compared with the sliding mode control. In addition, as can be seen from the errors of the attitude angles in fig. 9, 11 and 13, the adaptive sliding mode control has better anti-shake performance than the sliding mode control, and contributes to improving the stability of the control system.

In the second embodiment, the self-adaptive sliding mode controller designed in the step 4 is used for tracking the curve type motion trail of the vascular robot, and the control effect of the self-adaptive sliding mode controller is verified by comparing the curve type motion trail with the sliding mode controller designed in the step 3.

The setting of the robot simulation parameters is the same as in the first embodiment.

In order to verify the effectiveness of the self-adaptive sliding mode controller under different working conditions, the variable working conditions are dealt with, and in the embodiment, the curve type movement track is tracked.

Let the origin of the vascular robot be the origin of coordinates, design the motion trail curve of the vascular robot:

Posture compensation is carried out by utilizing a gravity-buoyancy compensator, simulation is carried out by a self-adaptive sliding mode control method, and parameters of a controller are set as follows:

Simulation results of tracking the curve-type motion trail of the vascular robot under the action of the sliding mode controller and the self-adaptive sliding mode controller are obtained through simulation, and the simulation results are shown in fig. 14-21. Fig. 14-15 are graphs of the motion trail of the vascular robot under the action of the slip-form controller and the adaptive slip-form controller. Compared with sliding mode control, the adaptive sliding mode control saves a large part of error convergence adjustment time in vascular robot track control, and the system response speed is faster. Fig. 16 to 21 are results of the attitude control of the vascular robot, and it is understood from the figures 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. The self-adaptive sliding mode control method is verified to have higher gesture tracking control precision. In addition, as can be seen from the error diagrams 17, 19 and 21 of the attitude angle, the adaptive sliding mode control has better anti-shake performance than the sliding mode control, and improves the stability of the control system.

Claims

1. A spiral vascular robot posture track integrated adaptive sliding mode tracking control method, characterized in that: the steps are:

Step 1: Use dual quaternions to simultaneously describe the posture and trajectory motion of the spiral vascular robot, and establish a posture-track integrated kinematic and dynamic model of the spiral vascular robot;

Step 2: When the spiral vascular robot moves in the blood, it will be affected by gravity-buoyancy, causing the actual motion trajectory to sink and deviate from the expected trajectory. A gravity-buoyancy compensator is designed to compensate for the sinking effect of gravity-buoyancy in the vertical direction.

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;

Step 4: Based on the sliding mode controller designed in step 3, design an adaptive sliding mode controller;

The specific process of establishing the attitude-orbit integrated kinematic and dynamic model described in step 1 is as follows:

The attitude and trajectory error of the vascular robot body coordinate system O _b X _b Y _b Z _b described by the unit dual quaternion relative to the desired coordinate system O _d X _d Y _d Z _d for:

in, and are the attitude trajectories of the desired coordinate system _OdXdYdZd and the body _coordinate system _ObXbYbZb of the vascular _robot expressed by _the unit dual quaternion relative to _the inertial coordinate system _OXYZ , respectively. “*” represents the multiplication of the dual quaternion _. represents the conjugate of a dual quaternion;

Velocity screw error of the 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 It is expressed in the coordinate system O _b X _b Y _b Z _b as:

in, is the velocity spinor of the vascular robot body coordinate system O _b X _b Y _b Z _b relative to the inertial coordinate system OXYZ in the robot body coordinate system O _b X _b Y _b Z _b , is the expression of the velocity spinor 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 , (·) ^* denotes the conjugate of the dual quaternion;

Then, by deduction, we can get The attitude-orbit integrated kinematics and dynamics equations are:

Where M ^b is the dual mass characteristic inertia matrix, represents the dual force spinor with the action point being the mass center of the vascular robot in the coordinate system O _b X _b Y _b Z _b , “×” represents the cross product operation of the dual quaternion, Represents an operation between a matrix and a dual quaternion. definition The operation is ε represents the dual unit; definition The operation is 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 Vascular robots in the blood are mainly driven by the total driving force Robot's gravity-buoyancy Fluid resistance The role of; total driving force The expression is:

in, It 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 pull generated by the magnetic gradient, n is the number of rotations 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 rise angle of the tail of the vascular robot, is the 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 drag coefficient perpendicular to the spiral tail, and ζ ₁₁ is the axial drag coefficient parallel to the tail of the spiral, and η is the blood viscosity coefficient, κ is the diameter of the spiral tail of the vascular robot;

Fluid resistance Fluid resistance generated by the impact of blood flow on the head and spiral tail of the vascular robot Composition, expressed as:

Robot gravity-buoyancy is gravity G and buoyancy The resultant force acting in the vertical direction is expressed as:

Among them, ρ is the density of the vascular robot, ρ _f is the density of blood, is the acceleration due to gravity;

Fluid resistance is needed to offset the effect of gravity-buoyancy on vascular robots and total driving force The combined effect is:

in In order to design the gravity-buoyancy compensator, the posture coordinate system p of the vascular robot is established. The origin of the coordinate system O _p is located at the center of gravity of the vascular robot. The x _p axis of the coordinate system is aligned with the actual direction of the vascular robot. The _zp axis is perpendicular to the actual direction of the vascular robot. The movement speed of the vascular robot to offset the influence of gravity and buoyancy.

According to formula (8), we get:

The expected movement speed of the vascular robot Decompose into horizontal components and the vertical component It can be seen that the expected movement speed of the vascular robot The horizontal component and The horizontal component Equal, that is, Vascular robot rotation angular velocity The relationship between the rotation angular velocity Ω ₁ and the three-dimensional rotating magnetic field is as follows:

According to the above analysis, we can get:

in, is a unit vector;

when When , according to equations (9) and (10), we can get:

when When Decomposed into two forces, namely the component parallel to the _xp axis and the component perpendicular to the _xp axis exist The direction has no component, and due to the uncertainty and variability of the magnetic field gradient direction, it is assumed that Acting on the x _p z _p plane and in the direction of the desired motion speed of the vascular robot Symmetric about the _xp axis, similar to The force analysis of can be decomposed into components parallel to the _xp axis and the component perpendicular to the _xp axis exist If the direction has no component, then:

in, and Respectively The numerical values projected on the _zp axis and _xp axis, and The numerical value of the projection on the z _p axis and the x _p axis, represent the unit vectors in the zp _- axis direction and the _xp- axis direction respectively; Substituting equations (10) and (13) into equation (9), we can obtain:

because Define the desired pitch angle of the vascular robot as Speed Projecting to the _xp axis and _zp axis, according to formula (14), we can get:

By analyzing the mechanical geometric relationship of the vascular robot's posture coordinate system p, we can obtain:

Where β is the angle at which the vascular robot is expected to tilt upward;

Similar to formula (16) Analysis, The following relationship can also be obtained:

According to formula (15), formula (16) and formula (17), we can get:

According to cos(θ+β)=cos(θ)cos(β)-sin(θ)sin(β), the angle β of the vascular robot output by the gravity-buoyancy compensator compared to the ideal pitch angle can be obtained from formula (18):

Through equations (15), (16) and (17), it can be obtained that the rotation angular velocity Ω ₁ of the three-dimensional rotating magnetic field output by the gravity-buoyancy compensator is:

The sliding mode controller based on gravity-buoyancy compensation described in step 3 is:

The expected motion trajectory of the vascular robot is [x _d (t), y _d (t), z _d (t)] ^T , and the initial rolling angular velocity of the robot is Then the expected yaw angle ψ and expected pitch angle θ of the vascular robot are:

Assume that the roll angular velocity of the vascular robot is The angular velocity of the three-dimensional rotating magnetic field is consistent with Ω _1. Under the action of the gravity-buoyancy compensator, the yaw angle ψ ^s , pitch angle θ ^s , and roll angular velocity of the vascular robot are for:

Finally, the expected motion trajectory of the vascular robot [x _d (t), y _d (t), z _d (t)] ^T and the posture angle of the vascular robot after gravity-buoyancy compensation are calculated. Perform dual quaternion transformation to obtain the motion description under the dual quaternion framework Thus, the dual quaternion motion of the desired coordinate system of the vascular robot is As input to the control system;

_The control goal of the vascular robot is to control the posture and trajectory error of _the _vascular robot body coordinate system _ObXbYbZb relative to _the desired _coordinate _system _OdXdYdZd Make the actual motion state of the vascular robot Asymptotically converges to the motion state in the desired coordinate system

Based on the kinematics and dynamics equations (3) and (4) of the vascular robot posture-track integration established in step 1, the following sliding mold surface is designed:

in, is the product operation of even numbers, is the controller parameter, And k _si > 0,

The designed sliding mode control law is:

in, is the sliding mode variable, sgn(·) is a sign function, and is the controller parameter, and k _εi ＞0, is the controller parameter, And k _ui ＞0, is a product operation of dual vectors, is the dual interference force;

The adaptive sliding mode controller described in step 4 is:

To facilitate the design of the adaptive sliding mode controller, the term with M ^b in equation (27) is expressed as follows:

in, is another way of expressing the dual mass characteristic inertia matrix M ^b ; And Π∈R _3×1 , Through the special property of dipole operation ε ² = 0, we can further obtain is a three-dimensional dual vector; due to the particularity of the dual mass characteristic inertia matrix M ^b, it is known that Π is easy to obtain, and about The calculation of can be derived from equations (29)-(31): For any x∈R _3×1 ,y∈R _3×1 , we have:

Among them, δ(x,y)＝δ ₁ (x)+δ ₂ (y), and δ ₁ (x), δ ₂ (y) are calculated as follows:

Due to the complex working environment and variable working content of the vascular robot, its mass and inertia are often uncertain and variable during the modeling process of the vascular robot, so the dual mass characteristic inertia matrix is and dual interference force Estimated value of The design parameter adaptive law is as follows:

in, is the controller parameter, is the controller parameter, and:

Substituting equations (32) and (33) into the sliding mode control law of equation (27), the adaptive sliding mode control law can be obtained as follows: