CN114019798B - Robot track tracking control method, magnetic medical robot and storage medium - Google Patents
Robot track tracking control method, magnetic medical robot and storage medium Download PDFInfo
- Publication number
- CN114019798B CN114019798B CN202111295113.5A CN202111295113A CN114019798B CN 114019798 B CN114019798 B CN 114019798B CN 202111295113 A CN202111295113 A CN 202111295113A CN 114019798 B CN114019798 B CN 114019798B
- Authority
- CN
- China
- Prior art keywords
- robot
- coordinate
- actual
- iteration
- motion parameters
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 238000005457 optimization Methods 0.000 claims abstract description 31
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 5
- 238000005516 engineering process Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000012360 testing method Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 229940079593 drug Drugs 0.000 description 1
- 230000005684 electric field Effects 0.000 description 1
- 239000013604 expression vector Substances 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 244000005700 microbiome Species 0.000 description 1
- 238000002324 minimally invasive surgery Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 238000002626 targeted therapy Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 238000004260 weight control Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The application relates to the field of robot track tracking control, and discloses a robot track tracking control method, a magnetic medical robot and a storage medium. The method comprises the following steps: acquiring actual motion trail coordinates and reference trail coordinates of the robot based on actual motion parameters; calculating coordinate errors of the actual track coordinates and the reference track coordinates; taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration; to optimize the motion parameters and control the robot motion. By the mode, the control precision and stability of the robot can be improved.
Description
Technical Field
The present application relates to the field of robot trajectory tracking control, and in particular, to a robot trajectory tracking control method, a magnetic medical robot, and a storage medium.
Background
Along with continuous integration of informatization and industrialization, intelligent industries represented by robot technologies are vigorous, and become an important mark for modern technological innovation. Medical treatment and robot combination also become an important plate in technological innovation. The range of medical robots is very wide and robots or robotics used in the full phase of medical treatment can be called medical robots. Medical robots are currently classified into surgical robots, rehabilitation robots, medical service robots, intelligent devices, and the like.
Among them, a micro-robot is often used in targeted therapy and minimally invasive surgery. With the development of micro-nano processing technology, micro robots are also being optimized continuously, and people are commonly called as artificial micro-nano robots at present. Inspired by free motion of microorganisms in nature, artificial micro-nano robots are widely focused and studied in recent years, can be effectively driven by means of electric fields, magnetic fields, optical fields and the like, and have wide application in the fields of noninvasive surgery, targeted drug transportation, biosensing detection and the like. In various control schemes, the magnetic control drive can precisely control the micro-nano robot in a wireless mode, and by changing the gradient and the direction of an external magnetic field, force and moment can be applied to the magnetic control micro-nano robot, so that the magnetic control micro-nano robot moves along a desired track.
Among the problems of real-time trajectory tracking control of magnetic micro-nano robots, pure tracking methods (Pure-Pure) and Stanley methods in trajectory tracking based on geometric methods are mostly adopted. The Pure-Pursuit method is a proportional controller for calculating the distance error from the current position to a certain target position. When the current detection distance is too large, the tracking performance is poor, and the track is easy to deviate; while the Stanley method takes into account angle errors and distance errors, it does not perform well on discontinuous paths. For a model-based predictive optimization control method, the speed of the optimization solution is to be improved.
Disclosure of Invention
The application mainly solves the technical problem of providing a robot track tracking method which can improve the control precision and stability of a robot.
In order to solve the technical problems, the application adopts a technical scheme that: the robot track tracking control method comprises the following steps:
acquiring actual track coordinates and reference track coordinates of the robot based on actual motion parameters; calculating coordinate errors of the actual track coordinates and the reference track coordinates; taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration; to optimize the motion parameters and control the robot motion.
Further, calculating the coordinate error of the actual track coordinate and the reference track coordinate includes obtaining the coordinate error based on the coordinate difference of the actual track coordinate and the reference track coordinate and the actual motion parameter.
Further, obtaining a coordinate error based on a coordinate difference value of the actual track coordinate and the reference track coordinate and the actual motion parameter, wherein the coordinate error comprises the steps of constructing a state coefficient matrix by the reference motion parameter, calculating a product of the state coefficient matrix and the coordinate difference value, and a product of the actual motion parameter and the control coefficient matrix; the sum of the two products is taken as the coordinate error.
Wherein the reference motion parameter isWherein v is r Representing the reference movement speed, w r Representing a reference angular velocity of motion; the state coefficient matrix is->The control coefficient matrix is +.>The coordinate error is +.>Wherein e 1 、e 2 、e 3 Representing the coordinate difference between the actual track coordinates and the reference track coordinates.
Further, acquiring an actual track coordinate and a reference track coordinate of the robot based on the actual motion parameter, wherein the acquiring comprises acquiring the actual track coordinate of the robot based on the actual motion parameter under each step length in a prediction range; calculating the coordinate errors of the actual track coordinate and the reference track coordinate, wherein the calculating comprises calculating the coordinate errors of the actual track coordinate and the reference track coordinate under each step length; the minimization of the coordinate error is taken as an iteration target, and the sum of the coordinate difference values in the coordinate errors under all step sizes and the minimization of the sum of the actual motion parameters in the control range are taken as the iteration target.
Further, the method includes the prediction horizon being greater than or equal to the control horizon.
Further, taking the minimization of the coordinate error as an iteration target, carrying out optimization iteration on the actual motion parameter of the robot to obtain an optimized motion parameter, and taking the actual motion parameter in a preset range, the optimized motion parameter in the preset range and the coordinate difference value in the preset range as constraint conditions.
Further, performing optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters, wherein the optimization iteration is performed on all the actual motion parameters of the robot in a control range to obtain an optimized motion parameter sequence; controlling the robot motion with the optimized motion parameters includes controlling the robot motion with the currently optimized motion parameters in the optimized motion parameter sequence.
In order to solve the problems, the application adopts another technical scheme that: there is provided a magnetic medical robot comprising a processor and a memory coupled to the processor, the memory having stored therein a computer program for executing the computer program to perform the method described above.
In order to solve the problems, the application adopts another technical scheme that: there is provided a computer readable storage medium having stored thereon program data for carrying out the above method when executed by a processor.
The beneficial effects of the application are as follows: different from the prior art, the application provides a robot track tracking control method. The method comprises the steps of obtaining actual motion track coordinates and reference track coordinates of a robot based on actual motion parameters; calculating coordinate errors of the actual track coordinates and the reference track coordinates; taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration; to optimize the motion parameters and control the robot motion. According to the method, the three-degree-of-freedom motion model and the track tracking error model are constructed, and the error model is controlled by adopting an acceleration optimization iteration control method, so that the convergence rate is increased. By the mode, the control precision and stability of the robot can be improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art. Wherein:
FIG. 1 is a schematic flow chart of an embodiment of a method for controlling tracking of a robot track;
FIG. 2 is a schematic flow chart of another embodiment of a robot trajectory tracking control method provided by the present application;
FIG. 3 is a schematic flow chart of another embodiment of a robot trajectory tracking control method provided by the present application;
FIG. 4 is a schematic view of an embodiment of a magnetic medical robot according to the present application;
FIG. 5 is a schematic diagram illustrating the structure of an embodiment of a computer-readable storage medium provided by the present application;
FIG. 6 is a graph of circular trace results provided by the present application;
FIG. 7 is a time chart of the circular trace tracking provided by the present application for each iteration;
FIG. 8 is a figure of the figure of eight trace results provided by the present application;
FIG. 9 is a time chart of the splayed trajectory tracking provided by the present application for each iteration.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application. It is to be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application. It should be further noted that, for convenience of description, only some, but not all methods and processes related to the present application are shown in the accompanying drawings. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.
The terms "comprising" and "having" and any variations thereof herein are intended to cover a non-exclusive inclusion. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those listed steps or elements but may include other steps or elements not listed or inherent to such process, method, article, or apparatus.
Reference herein to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment may be included in at least one embodiment of the application. The appearances of such phrases in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. Those of skill in the art will explicitly and implicitly appreciate that the embodiments described herein may be combined with other embodiments.
Referring to fig. 1, fig. 1 is a schematic flow chart of an embodiment of a robot trajectory tracking control method according to the present application. The robot track tracking control method in this embodiment specifically includes steps 11 to 14:
step 11: acquiring actual motion trail coordinates and reference trail coordinates of the robot based on actual motion parameters;
under the normal condition, the magnetic control micro robot is positioned by utilizing visual feedback or ultrasonic image positioning, the motion parameters of the robot in the actual motion process are obtained, the actual motion track of the robot is tracked, an actual motion track coordinate system is established, and the actual motion track coordinate of the robot is obtained; and simultaneously establishing a reference track coordinate system, and acquiring a reference track coordinate corresponding to the actual motion track coordinate.
Step 12: calculating coordinate errors of the actual track coordinates and the reference track coordinates;
the coordinate error represents the deviation between the actual track coordinate and the reference track coordinate, and the larger the deviation is, the more the movement of the robot deviates from the specified track, and the worse the control precision is; the smaller the deviation, the closer the robot motion is to the predetermined trajectory, and the higher the control accuracy.
Because the actual track is in the actual motion track coordinate system and the reference track is in the reference track coordinate system, the actual track and the reference track cannot be directly calculated, and therefore the actual track and the reference track need to be converted into the same coordinate system, and the coordinate error of the converted actual track coordinate and the coordinate error of the reference track coordinate need to be calculated.
Step 13: taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration;
the iterative algorithm is generally used to solve an optimization problem, and can iterate repeatedly until the optimal value most conforming to the condition appears, and stopping iterative calculation. The current iterative algorithm comprises a Newton iterative algorithm, an iterative closest point algorithm, a dichotomy iterative algorithm and the like.
The application provides a new method based on model predictive control and a hot start (wall-start) technology, so that a model can quickly and iteratively solve an optimization control problem. Firstly, a model predictive controller is designed based on an error model, and a hot start technology is combined to carry out rapid iterative solution. The stability of the robot is ensured due to the movement of the robot, so the new method based on the model predictive control and the hot start technology provided by the application is used for solving the problem of secondary planning by restraining the actual movement parameters of the robot. Common methods for solving the quadratic programming problem include an effective set method (Active Set Method), an interior point method (Interior Point Method) and the like. In order to make the iteration effect better and the iteration speed faster, a hot start technology is combined with an active set method, and an optimized value obtained after each iteration is used as an initial value of the next iteration to calculate so as to solve the problems.
The coordinate error is optimized and iterated by utilizing the optimized iteration function, so that the minimum coordinate error is obtained, namely, the actual motion track approaches to the reference motion track infinitely, so that the coordinate error can move according to the path specified by the reference motion track, and higher control precision is achieved. Besides the optimization iteration of the coordinate error, the optimization iteration is also needed for the actual motion parameters of the robot, and the motion parameters are continuously changed because the robot continuously moves and the motion track is mostly a curve. In order to make the robot motion smoother and reduce the severe shaking condition of the robot, optimization iteration is required to be carried out on the actual motion parameters to obtain the optimized motion parameters.
Step 14: to optimize the motion parameters and control the robot motion.
And controlling the robot to move according to the parameter information by utilizing the optimized motion parameters obtained by the optimized iteration solution. Wherein the optimized motion parameters include an actual motion speed and an angular speed of the robot.
Step 121 is included in step 12.
Step 121: and obtaining a coordinate error based on the coordinate difference value of the actual track coordinate and the reference track coordinate and the actual motion parameter.
The actual track coordinates are represented by actual motion parameters in an actual track coordinate system, and the actual track coordinates comprise an abscissa, an ordinate and an angular velocity. Each of the actual trajectory coordinates is represented by an actual movement speed and an actual movement angular speed.
Step 121 further includes step 1211.
Step 1211: constructing a state coefficient matrix by using the reference motion parameters, and calculating the product of the state coefficient matrix and the coordinate difference value, and the product of the actual motion parameters and the control coefficient matrix; the sum of the two products is taken as the coordinate error.
In the coordinate error model, a state coefficient matrix is constructed by using reference motion parameters, and the influence of a coordinate difference value on the coordinate error is controlled by using the state coefficient matrix; and controlling the actual motion of the robot by using the actual motion parameters, and controlling the control effect of the actual motion parameters on the robot by using the control coefficient matrix.
The principle of the three-degree-of-freedom kinematic model and the error model constructed by the application is described below by using a formula derivation process.
The inventors first constructed a three degree of freedom kinematic model. Let [ x, y, θ ] denote the state of the kinematic model under the actual motion trajectory coordinate system, [ v, w ] denote the actual motion velocity and angular velocity, and the motion model is expressed as:
wherein,,the derivatives of x, y and θ are shown as the motion speed of the horizontal axis, the motion speed of the vertical axis and the angular velocity of the robot when the robot moves under the actual motion track coordinate system under the current actual motion speed and angular velocity.
Let [ x ] r ,y r ,θ r ]The reference point in the reference trajectory motion is represented, and the speed and angular speed of the reference trajectory motion are represented as [ v ] r ,w r ]The motion model under the reference trajectory coordinate system is expressed as:
in the same way, the processing method comprises the steps of,represents x r ,y r ,θ r And the derivative of the above represents the motion speed of the horizontal axis, the motion speed of the vertical axis and the angular velocity of the robot when moving under the reference motion trajectory coordinate system.
The method comprises the steps of projecting the central position of an actual motion track of a robot into a reference track, and defining an error expression vector at the moment as follows:
wherein x is r ,y r ,θ r In the reference track coordinate system, x, y and θ are in the actual motion track coordinate system, so that the error can be transformed from the actual motion coordinate system to the reference track coordinate system, and the result is that:
deriving the formula, and simultaneously bringing a motion model under an actual motion trail coordinate system and a motion model under a reference motion trail coordinate system into an error formula to obtain an error model:
wherein the input control amount (u 1 ,u 2 ) The method comprises the following steps:
and (3) bringing the control quantity into an error model to obtain:
according to the linear control theory, linearizing the error model at the balance point to obtain:
the error model can be expressed as according to the above formulaDefinition Δe= [ e 1 ,e 2 ,e 3 ] T As a state quantity, Δu= [ v, w]For controlling quantity, ++>Is a state coefficient matrix>For controlling the coefficient matrix of the quantity. The state quantity represents the coordinate error of the actual motion trail of the robot and the reference motion trail, and the smaller the value is, the closer the actual motion trail is to the reference motion trail; the state coefficient matrix controls the influence of state quantity on the error model; the control amount indicates the actual movement speed and angular speed input by each model, the difference between the input value and the last input value is not too large, and the too large difference can cause the conditions of severe shaking of the robot, unstable movement, burrs on the movement track and the like; the control quantity coefficient matrix is weighted for the influence on the control quantity.
In embodiment 1, by acquiring actual motion trajectory coordinates of the robot based on actual motion parameters and reference coordinates; calculating coordinate errors of the actual track coordinates and the reference track coordinates; taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration; to optimize the motion parameters and control the robot motion. In the process of each iteration, the optimized value obtained by the current iteration is used as the initial value of the next iteration, and no additional optimized parameter and calculated amount are introduced, so that the convergence speed of the optimized iteration can be increased, and the control precision of the robot and the stability of the robot motion are improved.
Referring to fig. 2, fig. 2 is a schematic flow chart of another embodiment of a robot trajectory tracking control method according to the present application. The embodiment specifically includes steps 21 to 23:
step 21: acquiring actual track coordinates of the robot based on actual motion parameters in each step within a prediction range;
the application adopts a model predictive control method to optimize the control parameters of the iterative robot so as to control the robot. Firstly, setting a prediction range, wherein the prediction range comprises a plurality of prediction step sizes, and acquiring the predicted actual track coordinates of the robot under the control of the actual motion parameters under each prediction step size;
step 22: calculating the coordinate error of the actual track coordinate and the reference track coordinate under each step length;
and acquiring reference track coordinates in the same prediction range under a plurality of step sizes in the reference track, finding out reference track coordinates corresponding to the predicted actual track coordinates one by one, and calculating coordinate errors of the actual track coordinates and the reference track coordinates.
Step 23: the sum of the coordinate difference values in the coordinate errors under all the step sizes and the minimization of the sum of the actual motion parameters in the control range are taken as iteration targets.
And predicting the actual motion parameters to obtain the actual motion parameters in a plurality of step sizes in a control range, wherein the prediction range is larger than or equal to the control range. And adding the coordinate differences under all the step sizes in the prediction range to obtain a total prediction error, adding the prediction control coefficients under all the step sizes in the control range to obtain a total prediction control quantity, and taking the minimization of the sum of the two as an iteration target, which is equivalent to taking the minimum total prediction error and the minimum total prediction control quantity as the iteration target to obtain an optimal control sequence when the sum of the two is minimized. The optimal control sequence comprises an optimal control quantity at the current moment and an optimal predicted control quantity at the current moment, and each control quantity comprises an actual movement speed and an angular speed.
The iterative process will be expressed in terms of a formula in combination with an explanation.
And (3) according to a model predictive control theory and a coordinate error model, adopting an effective set method with constraint to carry out optimization iteration on the control speed and the angular speed of the robot. First, an objective function and constraints are constructed as follows:
s.t.Δu low ≤Δu(k+j-1)≤Δu up
ΔΔu low ≤ΔΔu(k+j-1)≤ΔΔu up
e low ≤Δe(k+j)≤e up
wherein k represents the moment, Δe (k+j) represents the coordinate difference between the actual motion trajectory and the reference motion trajectory at the (k+j) th moment, Δu (k+j-1) represents the control amount at the (k+j-1) th moment, and each time the summation iteration is started by using the control amount at the current moment and the predicted coordinate error at the next moment; n (N) p Represent the prediction horizon, N u Representing a control range, both of which are super parameters; q and R are constant coefficient matrixes, and are respectively used as a coordinate difference value and a weight control matrix of the control quantity; u (u) k+1 ,…,u k+Nu Representing a control sequence to be finally solved; deltau in constraint low ,Δu up The lower control limit and the upper control limit, respectively, representing the control amount, ΔΔu (k+j-1) represents the optimal control amount, ΔΔu low ,ΔΔu up Represents the optimal lower control limit and the optimal upper control limit, and is the same as low ,e up Representing a lower coordinate difference limit and an upper coordinate difference limit.
Assume that the increment of the optimal control sequence obtained at time k-1 is:
ΔΔu * (k-1)＝[ΔΔu * (k-1|k-1),…,ΔΔu * (k+N u -2|k-1)]
and combining a hot start technology, removing the optimal control increment at the moment k-1, combining the rest predicted control increments into a new control sequence, and inputting the new control sequence as an initial value of the next optimization iteration into an objective function, wherein the optimal control increment is the first increment in the optimal control sequence, and the optimal control increment is used for controlling the motion of the robot as the control quantity of the robot at the current moment. Thus, the initial input for the optimization iteration at time k is:
ΔΔu shift (k)＝[ΔΔu * (k|k-1),…,ΔΔu * (k+N u -2|k-1),ΔΔu * (k+N u -2|k-1)]
since the initial value sequence length of each iteration is N u Therefore, the last prediction control amount at time k-1 is copied once, and the whole is taken as the initial value of the optimization iteration at time k.
Because the initial control sequence of each iteration is the optimal control sequence of the last iteration, the iteration convergence speed is increased, and meanwhile, the difference between the optimal control increments obtained after each iteration is not very large, so that the motion stability of the robot is ensured.
Referring to fig. 3, fig. 3 is a flowchart illustrating another embodiment of a robot trajectory tracking control method according to the present application. The embodiment specifically includes steps 31 to 32:
step 31: performing optimization iteration on all actual motion parameters of the robot in a control range to obtain an optimized motion parameter sequence;
the related content derivation is given in the formula, and the optimization iteration is carried out by inputting the actual motion parameter sequence each time so as to obtain an optimized motion parameter sequence;
step 32: and controlling the robot to move with the current optimized motion parameters in the optimized motion parameter sequence.
In the optimized motion parameter sequence, the first optimal control increment of the current sequence is selected each time to serve as a control parameter for controlling the motion of the robot, and the robot continues to move according to the control parameter.
According to embodiments 2 and 3, it is possible to update the optimal control sequence by performing optimization iteration each time on the sum of the control amounts and the coordinate difference value of the actual motion trajectory and the reference motion trajectory. The optimal control sequence iterated at the previous moment is used as the slave initial control sequence iterated at the current moment, so that the iteration convergence speed is effectively increased, the stable motion of the robot is controlled, and meanwhile, the track tracking precision of the robot is improved.
Referring to fig. 4, fig. 4 is a schematic structural diagram of an embodiment of a magnetic medical robot according to the present application.
Included in the magnetic medical robot 100 are a processor 110 and a memory 120. Wherein the processor 110 and the memory 120 are coupled. The memory 120 stores therein a computer program for executing the robot trajectory tracking control method described above.
Referring specifically to fig. 5, fig. 5 is a schematic structural diagram of an embodiment of a computer readable storage medium according to the present application.
Included in computer readable storage medium 200 is program data 210. The program data 210, when executed by a processor, may implement the robot trajectory tracking control method described above.
To fully illustrate the advantages of the present application, the present application tracks a circular track and a splayed track, respectively, having a radius of 10 cm. Referring first to fig. 6, fig. 6 is a graph of a circular trace result provided by the present application. The tracking test was performed at two sampling time intervals, ts=0.01 and ts=0.05, respectively. According to fig. 6, it can be observed that, from the view of the track tracking effect of the lowest right measurement of the circle, the track tracking error is larger by using the active set method (the active set method), the distance between the track tracking error and the reference track is longer, and the track tracking control precision is higher by using the active set method based on the hot start to be closer to the reference track.
Fig. 7 is a time chart of each iteration in circular trajectory tracking provided by the present application. As can be seen from an examination of fig. 7, the time taken to reach convergence is long when the iteration is performed using the active set method, and the convergence speed of each iteration is obviously increased by the active set method based on hot start.
Meanwhile, the application tests in another reference track, referring to fig. 8, fig. 8 is a figure-eight track tracking result diagram provided by the application. Similarly, it can be observed that the track tracking effect based on the active set method is poor and the deviation between the track tracking effect and the reference track is large at the position with large change of the reference track, and the track tracking effect based on the active set method based on the hot start is good and basically has no deviation or has small deviation with the reference track. Referring to fig. 9, fig. 9 is a time chart of the splayed track tracking provided by the present application for each iteration, and it can also be found that the convergence time of each iteration of the active set method based on hot start is smaller than that of the active set method.
Through the two tests, the application has the advantages of high track tracking precision, robot running stability, high optimization iteration convergence speed and the like.
Different from the prior art, the application provides a robot track tracking control method. Firstly, acquiring actual motion trail coordinates and reference trail coordinates of a robot based on actual motion parameters; calculating coordinate errors of the actual track coordinates and the reference track coordinates; taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration; to optimize the motion parameters and control the robot motion. According to the three-degree-of-freedom kinematic model and the error model, the application adopts the model predictive control theory to construct the iterative optimization objective function, and optimizes and iterates the predictive coordinate error sum and the predictive control quantity sum. The optimized motion parameter sequence obtained by each iteration optimization is used as an initial motion sequence of the next time optimization iteration, so that the convergence rate of iterative computation is increased, and meanwhile, the track tracking control precision of the robot is improved, so that the motion state of the robot is more stable.
The foregoing description is only illustrative of the present application and is not intended to limit the scope of the application, and all equivalent structures or equivalent processes or direct or indirect application in other related technical fields are included in the scope of the present application.
Claims (7)
1. A trajectory tracking control method of a robot, the method comprising:
acquiring actual track coordinates and reference track coordinates of the robot based on actual motion parameters;
constructing a state coefficient matrix by using reference motion parameters, and calculating the product of the state coefficient matrix and a coordinate difference value, wherein the product of the actual motion parameters and a control coefficient matrix; taking the sum of the two products as a coordinate error; wherein the reference motion parameter isWherein v is r Representing the reference movement speed, w r Representing a reference angular velocity of motion; the shape isThe state coefficient matrix isThe control coefficient matrix is->The coordinate error is->Wherein e 1 、e 2 、e 3 Representing a coordinate difference of the actual track coordinate and the reference track coordinate;
taking the minimization of the coordinate error as an iteration target, and carrying out optimization iteration on the actual motion parameters of the robot to obtain optimized motion parameters; wherein, the initial value of each iteration adopts the optimized value of the last iteration;
and controlling the robot to move according to the optimized motion parameters.
2. The trajectory tracking control method according to claim 1, wherein the acquiring actual trajectory coordinates and reference trajectory coordinates of the robot based on actual motion parameters includes:
acquiring actual track coordinates of the robot based on actual motion parameters under each step length in a prediction range;
the calculating the coordinate error of the actual track coordinate and the reference track coordinate includes:
calculating the coordinate error of the actual track coordinate and the reference track coordinate under each step length;
the minimizing the coordinate error is used as an iteration target, and comprises the following steps:
the sum of the coordinate difference values in the coordinate errors under all the step sizes and the minimization of the sum of the actual motion parameters in the control range are taken as iteration targets.
3. The trajectory tracking control method according to claim 2, characterized in that the prediction range is greater than or equal to the control range.
4. The trajectory tracking control method according to claim 2, wherein the optimizing iteration of the actual motion parameters of the robot with the minimization of the coordinate error as an iteration target, to obtain optimized motion parameters, further comprises:
and taking the actual motion parameters within a preset range, the optimized motion parameters within the preset range and the coordinate difference values within the preset range as constraint conditions.
5. The trajectory tracking control method according to claim 1, wherein the performing optimization iteration on the actual motion parameter of the robot to obtain an optimized motion parameter includes:
performing optimization iteration on all actual motion parameters of the robot in a control range to obtain an optimized motion parameter sequence;
the controlling the robot to move with the optimized motion parameters comprises:
and controlling the robot to move according to the current optimized motion parameters in the optimized motion parameter sequence.
6. A magnetic medical robot comprising a processor and a memory coupled to the processor, the memory having a computer program stored therein, the processor being configured to execute the computer program to implement the method of any of claims 1-5.
7. A computer readable storage medium, characterized in that the computer readable storage medium has stored therein program data for implementing the method according to any of claims 1-5 when being executed by a processor.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111295113.5A CN114019798B (en) | 2021-11-03 | 2021-11-03 | Robot track tracking control method, magnetic medical robot and storage medium |
| PCT/CN2021/137811 WO2023077618A1 (en) | 2021-11-03 | 2021-12-14 | Robot trajectory tracking control method, magnetic medical robot and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111295113.5A CN114019798B (en) | 2021-11-03 | 2021-11-03 | Robot track tracking control method, magnetic medical robot and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114019798A CN114019798A (en) | 2022-02-08 |
| CN114019798B true CN114019798B (en) | 2023-08-11 |
Family
ID=80060390
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111295113.5A Active CN114019798B (en) | 2021-11-03 | 2021-11-03 | Robot track tracking control method, magnetic medical robot and storage medium |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN114019798B (en) |
| WO (1) | WO2023077618A1 (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115494751A (en) * | 2022-05-09 | 2022-12-20 | 深圳先进技术研究院 | Controller training method, trajectory tracking method, terminal device, and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109885052A (en) * | 2019-02-26 | 2019-06-14 | 华南理工大学 | Error model forecast Control Algorithm based on omni-directional mobile robots Kinematic Model |
| CN110948504A (en) * | 2020-02-20 | 2020-04-03 | 中科新松有限公司 | Normal constant force tracking method and device for robot machining operation |
| CN111324146A (en) * | 2020-03-09 | 2020-06-23 | 河海大学常州校区 | Trajectory tracking control method of underwater inspection robot |
| CN111872933A (en) * | 2019-11-25 | 2020-11-03 | 浙江大学宁波理工学院 | SCARA robot trajectory tracking control method based on improved quadratic iterative learning control |
| CN112650217A (en) * | 2020-12-03 | 2021-04-13 | 南京理工大学 | Robot trajectory tracking strategy dynamic optimization method based on evaluation function |
| CN113146600A (en) * | 2021-03-25 | 2021-07-23 | 中山大学 | Flexible robot trajectory planning method and device based on kinematics iterative learning control |
| CN113342003A (en) * | 2021-07-14 | 2021-09-03 | 北京邮电大学 | Robot track tracking control method based on open-closed loop PID (proportion integration differentiation) type iterative learning |
| CN113485389A (en) * | 2021-08-03 | 2021-10-08 | 艾乐尔机器人科技(南京)有限公司 | Underwater robot three-dimensional trajectory tracking control method based on self-adaptive prediction |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6632487B2 (en) * | 2016-07-13 | 2020-01-22 | キヤノン株式会社 | Continuum robot, method of correcting kinematics, and control method of continuum robot |
| CN112947430B (en) * | 2021-02-03 | 2022-07-15 | 浙江工业大学 | Intelligent trajectory tracking control method for mobile robot |
-
2021
- 2021-11-03 CN CN202111295113.5A patent/CN114019798B/en active Active
- 2021-12-14 WO PCT/CN2021/137811 patent/WO2023077618A1/en unknown
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109885052A (en) * | 2019-02-26 | 2019-06-14 | 华南理工大学 | Error model forecast Control Algorithm based on omni-directional mobile robots Kinematic Model |
| CN111872933A (en) * | 2019-11-25 | 2020-11-03 | 浙江大学宁波理工学院 | SCARA robot trajectory tracking control method based on improved quadratic iterative learning control |
| CN110948504A (en) * | 2020-02-20 | 2020-04-03 | 中科新松有限公司 | Normal constant force tracking method and device for robot machining operation |
| CN111324146A (en) * | 2020-03-09 | 2020-06-23 | 河海大学常州校区 | Trajectory tracking control method of underwater inspection robot |
| CN112650217A (en) * | 2020-12-03 | 2021-04-13 | 南京理工大学 | Robot trajectory tracking strategy dynamic optimization method based on evaluation function |
| CN113146600A (en) * | 2021-03-25 | 2021-07-23 | 中山大学 | Flexible robot trajectory planning method and device based on kinematics iterative learning control |
| CN113342003A (en) * | 2021-07-14 | 2021-09-03 | 北京邮电大学 | Robot track tracking control method based on open-closed loop PID (proportion integration differentiation) type iterative learning |
| CN113485389A (en) * | 2021-08-03 | 2021-10-08 | 艾乐尔机器人科技(南京)有限公司 | Underwater robot three-dimensional trajectory tracking control method based on self-adaptive prediction |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2023077618A1 (en) | 2023-05-11 |
| CN114019798A (en) | 2022-02-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110524544A (en) | A kind of control method of manipulator motion, terminal and readable storage medium storing program for executing | |
| US7495537B2 (en) | Method and apparatus for dynamic magnetic field control using multiple magnets | |
| CN112417755A (en) | Master-slave mode surgical robot track prediction control method | |
| Qiu et al. | Disturbance observer based adaptive model predictive control for uncalibrated visual servoing in constrained environments | |
| WO2018019111A1 (en) | Robot and joint motion control method and device therefor | |
| CN110861088A (en) | Motion optimization method of redundant degree of freedom robot | |
| CN116533249A (en) | Mechanical arm control method based on deep reinforcement learning | |
| Ko et al. | Trajectory following for a flexible probe with state/input constraints: An approach based on model predictive control | |
| CN114019798B (en) | Robot track tracking control method, magnetic medical robot and storage medium | |
| CN116661469B (en) | Robot track error correction method and system | |
| CN109866222A (en) | A kind of manipulator motion planning method based on longicorn palpus optimisation strategy | |
| Yu et al. | Modeling electromagnetic navigation systems for medical applications using random forests and artificial neural networks | |
| Liu et al. | Model predictive control of magnetic helical swimmers in two-dimensional plane | |
| Das Sharma et al. | Harmony search-based hybrid stable adaptive fuzzy tracking controllers for vision-based mobile robot navigation | |
| Huang et al. | 4-dof visual servoing of a robotic flexible endoscope with a predefined-time convergent and noise-immune adaptive neural network | |
| Carlési et al. | Nonlinear model predictive running control of kangaroo robot: A one-leg planar underactuated hopping robot | |
| Fahimi et al. | An alternative closed-loop vision-based control approach for Unmanned Aircraft Systems with application to a quadrotor | |
| Qi et al. | Model predictive manipulation of compliant objects with multi-objective optimizer and adversarial network for occlusion compensation | |
| Liu et al. | Efficient reinforcement learning control for continuum robots based on inexplicit prior knowledge | |
| CN117601120A (en) | Adaptive variable impedance control method and device, electronic equipment and storage medium | |
| Chen et al. | QPSO-MPC based tracking algorithm for cable-driven continuum robots | |
| Korayem et al. | Analysis and experimental study of non-holonomic mobile manipulator in presence of obstacles for moving boundary condition | |
| Eberhard et al. | Sensor data fusion for the localization and position control of one kind of omnidirectional mobile robots | |
| Seekircher et al. | A closed-loop gait for humanoid robots combining LIPM with parameter optimization | |
| CN116476078B (en) | Laser radar-based stable target tracking method for snake-shaped robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |