A robot control method based on trend perception

Abstract

The application provides a robot control method based on trend awareness, which is used for treating a subject, and comprises the following steps: S1, acquiring a target motion trajectory of at least one mechanical arm, and acquiring an actual trend parameter of a treatment site; the target motion trajectory of the at least one mechanical arm is the same as a target motion trajectory of the treatment site corresponding to the mechanical arm; S2, applying an auxiliary force field according to the target motion trajectory and the actual trend parameter; the robot provides auxiliary control for the treatment site of the subject based on the auxiliary force field, and corrects an actual motion trajectory of the treatment site; and S3, enabling the at least one mechanical arm to move according to the target trajectory based on admittance control. The robot control method based on trend awareness can improve the cooperation among doctors, robots and subjects, and realize soft control.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of mechanical control, and particularly relates to a robot control method based on trend perception. BACKGROUND

[0002] With the development of robot technology, at present, a large number of robots have been applied to the medical field to improve the efficiency, accuracy and safety of medical services. Among them, medical robots include surgery-assisted, remote medical, assisted rehabilitation therapy, etc., and robots for assisting surgery or treatment are particularly common.

[0003] Medical robots usually realize accurate posture control through continuous trajectory control, torque control, intelligent control and other ways, and through high-precision control of mechanical arm movement, the robot can realize accurate treatment operation, and its research and development embodies the cross-fusion of multiple disciplines such as machinery, medicine and computer, and its research and development details embody the development trend of quantitative, standardized "medical and engineering combination".

[0004] However, the existing medical robot can usually only move according to a fixed trajectory, or intelligently identify the position coordinates to be moved at the next moment through image recognition, without considering the displacement of the subject to be treated, and if the robot is applied to a vehicle, when the vehicle bounces, the body posture and attitude of the subject to be treated will change, and if the medical robot still moves according to the fixed trajectory or only adjusts the position of the mechanical arm end according to the image recognition result, the treatment accuracy will greatly decrease, thereby reducing the treatment effect.

[0005] Based on the above, the present application provides a technical solution to solve the above technical problems. SUMMARY

[0006] In view of the scene that the treatment accuracy is reduced when the medical robot in the prior art is used in an unstable and large environment, the present application provides a robot control method based on trend perception, the robot comprising at least a plurality of mechanical arms, each mechanical arm holding a treatment instrument at the mechanical arm end to treat at least one treatment site of a subject, comprising the following steps:

[0007] Step S1, acquiring a target motion trajectory of at least one mechanical arm, and acquiring an actual trend parameter of the treatment site, the target motion trajectory of the at least one mechanical arm being the same as the target motion trajectory of the treatment site corresponding to the mechanical arm;

[0008] Step S2, applying an auxiliary force field according to the target motion trajectory and the actual trend parameter, the robot providing auxiliary control for the treatment site of the subject based on the auxiliary force field, and correcting the actual motion trajectory of the treatment site;

[0009] Step S3, let at least one of the mechanical arms move according to the target trajectory based on admittance control.

[0010] In one specific embodiment of the present application, in step S3, the admittance control comprises:

[0011]

[0012] wherein F ext ∈R 6×1 is the combination of the interaction force and the torque between the treatment site and the mechanical arm end, assist ∈R 6 ×1 is the combination of the interaction force and the interaction torque, x out ∈R 6×1 and x des ∈R 6×1 are the actual and desired poses of the mechanical arm end to be controlled respectively, M is the mass matrix, D is the damping matrix, K is the stiffness matrix, x, y, z and rx, ry, rz are the position and Euler angles of the mechanical arm end respectively, denotes the joint angular velocity, J(q)∈R 6×6 is the Jacobian matrix of the mechanical arm, is the joint angular velocity of the input to the robot force, is the angular velocity of the mechanical arm end to be controlled.

[0013] In one specific embodiment of the present application, the admittance control further comprises:

[0014]

[0015] F ah = F at + F an + F h ,

[0016]

[0017] wherein F an ∈R 3×1 and F at ∈R 3×1 are the normal auxiliary force and the tangential auxiliary force respectively, F h ∈R 3×1 and T h ∈R 3×1 are the interaction force and the interaction torque respectively, F ah ∈R 3×1 is the resultant force of the auxiliary force and the interaction force, M L ∈R 3×3 and D L ∈R 3×3respectively, are the mass matrix and the damping matrix of the position admittance controller; is the linear velocity of the end of the robot arm that needs to be controlled; θ ∈R 3×3 and D θ ∈R 3×3 respectively, are the stiffness matrix and the damping matrix of the attitude admittance controller, θ∈R 3×1 is the actual Euler angle of the end of the robot arm, θ d ∈R 3×1 represents the target Euler angle.

[0018] In one specific embodiment of the present application, the actual trend parameters of the treatment site at least include: treatment site position, tangential motion capability, normal motion capability, normal motion trend, and the auxiliary force field includes a normal auxiliary force field and a tangential auxiliary force field;

[0019] Wherein, the tangential direction represents a motion direction tangent to the current position of the target motion trajectory and the same as the motion direction of the target motion trajectory, and the normal direction is perpendicular to the tangential direction.

[0020] In one specific embodiment of the present application, the auxiliary force field in step S2 includes:

[0021] Step S2.1, obtaining the treatment site position and the correction mode corresponding to the treatment site position;

[0022] Step S2.2, obtaining the tangential motion capability, the normal motion capability, and the normal motion trend;

[0023] Step S2.3, determining the normal auxiliary force field according to the normal motion trend and the normal motion capability;

[0024] Step S2.4, determining the tangential auxiliary force field according to the tangential motion capability;

[0025] Step S2.5, correcting the actual motion trajectory of the treatment site according to the normal auxiliary force field and the tangential auxiliary force field.

[0026] In one specific embodiment of the present application, step S2.1 includes: dividing the area around the target motion trajectory into a first control area, a second control area, and a third control area according to the target motion trajectory;

[0027] Wherein:

[0028] When 0≤d≤R s , the treatment site is located in the first control area, and the trajectory of the treatment site is corrected based on the first correction mode;

[0029] When R s ≤d≤R mIf d > R, the treatment site is located in the second control region, and the trajectory of the treatment site is corrected based on the second correction mode;

[0030] When d > R m If d > R, the treatment site is located in the third control region, and the trajectory of the treatment site is corrected based on the third correction mode;

[0031] d represents the distance between the treatment site and the target motion trajectory, R s represents the boundary between the first control region and the second control region, R m represents the boundary between the second control region and the third control region, R m > R s .

[0032] In one specific embodiment of the present application, the distance between the treatment site and the target motion trajectory is the Euclidean distance of the two:

[0033] where the condition needs to be met

[0034] The correction mode is: F an = F an1 + F D ,

[0035]

[0036] F at = K at F atmax t,

[0037]

[0038] where α is the curvature of the point with the minimum Euclidean distance on the target motion trajectory, U is the set of space points that the end of the robot arm can reach, F an represents the normal auxiliary force, P N (x N ,y N ,z N ) represents the target motion trajectory, P act (x a ,y a ,z a ) represents the position of the treatment site, F an1 ∈ R 3×1 and F D ∈ R 3×1 respectively represent the stiffness term and the damping term of the normal auxiliary force, where the damping term F D is to ensure that the normal auxiliary force decays with a damping coefficient K D ∈ R 3×3 , and the normal auxiliary force field in the second control region is Kan The normal auxiliary force field in the third control region increases with the deviation at a speed of a·K an , a is a change coefficient, n∈R 3×1 represents the direction of the normal force, F atmax ∈R 3×1 is the maximum tangential auxiliary force, K at is a tangential auxiliary coefficient, t∈R 3×1 represents the direction of the tangential force.

[0039] In one specific embodiment of the present application, the step S2.3 comprises:

[0040] The normal auxiliary force field

[0041] NCI=NMT·ER,

[0042]

[0043]

[0044] β represents a normal motion trend coefficient, NCI represents the trajectory following ability of the treatment site, ER represents a weighted average error index, NMT represents a normal motion trend, K an_max and K an_min are the upper limit and lower limit of the normal stiffness coefficient, e p represents an error, KW i represents a time weight coefficient, KW max and KW min represent the maximum weight coefficient and the minimum weight coefficient, i represents the time frame number in the current time window, T all is the frame number of the time window, m·ΔT represents the moving time window size, and ΔT is the control period of the robot controller.

[0045] In one specific embodiment of the present application, the normal motion trend evaluation comprises:

[0046] The normal motion trend

[0047] is the motion direction of the target motion trajectory reference point corresponding to the current position, represents the interaction force of the treatment site, F ha ∈R 3×1 is the force of the interaction force F h of the treatment site decomposed to the normal, F ht ∈R 3×1 is the force of the interaction force F h decomposed to the tangential, F ha_maxThe maximum normal force output to the treatment site.

[0048] In one specific embodiment of the present invention, the tangential motion capability assessment includes:

[0049]

[0050] K at =g·TPI,

[0051]

[0052]

[0053] TP and TPI represent the tangential interaction force and transformation rate along the target motion trajectory at the treatment site, respectively, and dF ht_max K represents the upper limit of the rate of change of tangential interaction force. at denoted by , where g represents the tangential auxiliary force field coefficient and g represents the tangential motion tendency coefficient.

[0054] The present invention can bring at least one of the following beneficial effects: The present invention proposes a trend-aware robot control method, which can analyze the current movement trend of the subject and apply an adaptive auxiliary force to correct the position of the subject according to the position of the treatment site and the movement trend, thereby improving the accuracy of treatment and being effectively applied in unstable environments; In addition, compliant admittance control is introduced, enabling the medical robot to safely and effectively conduct safe physical interactions with the subject and doctor. Attached Figure Description

[0055] The preferred embodiments will now be described in a clear and easy-to-understand manner, with reference to the accompanying drawings, to further explain the above-mentioned characteristics, technical features, advantages, and implementation methods.

[0056] Figure 1 This is a schematic diagram illustrating the steps of a trend-aware robot control method proposed in this invention.

[0057] Figure 2 This is a schematic diagram of the flexible control based on admittance in this invention;

[0058] Figure 3 This refers to the division of the first control region, the second control region, and the third control region in this invention;

[0059] Figure 4 This is a schematic diagram showing the relationship between the actual motion trajectory and the applied auxiliary force field in this invention. Detailed Implementation

[0060] The various aspects of the present invention will be further described in detail below.

[0061] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. Methods and materials similar or equivalent to those described herein can be used in the practice of the present application.

[0062] The following terms are explained.

[0063] Unless otherwise defined, or specified and limited herewith, the term "or" as used in the specification includes "and", that is, unless otherwise indicated, a functionally logical "OR" is a subset of a functionally logical "AND".

[0064] It will be understood that, although the terms "first", "second", etc. can be used herein to describe various elements, these elements should not be limited by these terms. The terms are only used to distinguish one element from another. Thus, a first element could be termed a second element without departing from the teachings of the present application.

[0065] In the present application, the terms "comprising", "containing" or "including" mean that various components can be used together in a mixture or composition of the present application. Thus, the term "consisting essentially of is included in the terms "comprising", "containing" or "including".

[0066] Unless otherwise defined, or specified and limited herewith, the terms "connected", "coupled", "connected" in the present application should be interpreted broadly, for example, can be fixed connection, can be connected through intermediate medium, can be internal connection of two elements or interaction relationship of two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

[0067] For example, if an element (or component) is referred to as being "on" another element, "coupled with" another element, or "connected with" another element, it can be directly on, coupled with, or connected with the other element, or one or more intervening elements can be present. In contrast, if it is stated herein that an element is "directly on", "directly coupled with", or "directly connected with" another element, it means that no intervening element is present. Other words used to describe the relationship between elements should be interpreted similarly, such as "between" and "directly between", "attached" and "directly attached", "adjacent" and "directly adjacent", and the like.

[0068] It should be further understood that the terms "front", "back", "right", "left", "up" and "down" used in the following description are words of convenience and are not to be construed as limiting terms. Unless otherwise defined, all terms used in disclosing elements of structure limitations shall be construed as taking their ordinary meaning to one of ordinary skill in the art unless their meaning is more clearly oriented within the present application.

[0069] Additional aspects of the application will be apparent to those of ordinary skill in the art in view of the disclosure herein.

[0070] In order to more clearly illustrate the technical solutions of the embodiments of the present application or the prior art, specific embodiments of the present application will be described below with reference to the drawings. Obviously, the drawings in the following description are only some embodiments of the present application, and those of ordinary skill in the art can also obtain other drawings and embodiments from these drawings without creative labor.

[0071] It should be further understood that the terms "front", "back", "right", "left", "up" and "down" used in the following description are words of convenience and are not to be construed as limiting terms. Unless otherwise defined, all terms used in disclosing elements of structure limitations shall be construed as taking their ordinary meaning to one of ordinary skill in the art unless their meaning is more clearly oriented within the present application.

[0072] Embodiments

[0073] For the scene that the medical robot in the prior art is used in an unstable and large environment, the treatment accuracy is reduced, as shown in the prior art, the present application provides a robot control method based on trend perception, the robot includes at least a plurality of mechanical arms, each mechanical arm holds a treatment instrument at the end of the mechanical arm to treat at least one treatment site of a subject, including the following steps: Figure 1

[0074] Step S1, obtaining a target motion trajectory of at least one mechanical arm, obtaining an actual trend parameter of a treatment site, the target motion trajectory of the at least one mechanical arm and the target motion trajectory of the treatment site corresponding to the mechanical arm are the same;

[0075] Step S2, according to the target motion trajectory and the actual trend parameter, an auxiliary force field is applied, the robot provides auxiliary control for the treatment site of the subject based on the auxiliary force field, and corrects the actual motion trajectory of the treatment site;

[0076] ​Step S3, let at least one of the mechanical arms move according to the target trajectory based on the admittance control.

[0077] Preferably, the robot includes two mechanical arms. In a specific embodiment, the medical robot is a moxibustion robot, which assists or performs moxibustion treatment, including determining the location of acupoints (i.e. treatment sites), controlling the burning process of moxa sticks, including ignition, burning speed, burning time and other parameters, and accurately holding the burning moxa stick by the end of the mechanical arm according to the user's instructions, and applying precise heat stimulation to the treatment acupoint.

[0078] In a preferred embodiment of the present application, Figure 2 A schematic diagram of admittance-based flexible control is shown. In order to achieve the mass-damper-spring interaction effect between the user and the robot during interaction, in step S3, the admittance control in the Cartesian space includes:

[0079]

[0080] For a multi-degree-of-freedom moxibustion robot system, the joint speed is used to control the movement of the mechanical arm, which needs to calculate the required joint angular velocity, including:

[0081]

[0082] where F ext ∈R 6×1 is the combination of interaction force and torque between the treatment site and the end of the mechanical arm, F assist ∈R 6 ×1 is the combination of interaction force and interaction torque, x out ∈R 6×1 and x des ∈R 6×1 are the actual required control end-of-arm pose and desired pose, respectively, M is the mass matrix, D is the damping matrix, K is the stiffness matrix, x, y, z and rx, ry, rz are the position and Euler angles of the end of the mechanical arm, indicates the joint angular velocity, J(q)∈R 6×6 is the Jacobian matrix of the mechanical arm, is the joint angular velocity of the input force to the robot, is the required control end-of-arm angular velocity.

[0083] It should be understood that in the admittance control, since the subject's motion needs to be considered, the desired spatial linear velocity and the desired spatial linear acceleration are both 0, and there is interaction between the end of the mechanical arm and the user, the pose of the end of the mechanical arm cannot be completely fixed, and the end of the mechanical arm cannot be controlled, so the target Euler angle θd ∈R 3×1 , the admittance control further comprises:

[0084]

[0085] F ah = F at +F an +F h ,

[0086]

[0087] wherein F an ∈R 3×1 and F at ∈R 3×1 are normal auxiliary force and tangential auxiliary force respectively, F h ∈R 3×1 and T h ∈R 3×1 are interaction force and interaction torque respectively, F ah ∈R 3×1 is the resultant force of auxiliary force and interaction force, M L ∈R 3×3 and D L ∈R 3×3 are mass matrix and damping matrix of the position admittance controller respectively; is the linear velocity of the end of the robot arm that needs to be controlled; K θ ∈R 3×3 and D θ ∈R 3×3 are stiffness matrix and damping matrix of the attitude admittance controller respectively, θ∈R 3×1 is the actual Euler angle of the end of the robot arm, θ d ∈R 3×1 represents the target Euler angle.

[0088] Preferably, the actual trend parameters of the treatment site at least include: treatment site position, tangential motion ability, normal motion ability, normal motion trend, and the auxiliary force field comprises a normal auxiliary force field and a tangential auxiliary force field.

[0089] wherein the tangential direction represents a motion direction that is tangent to the current position of the target motion trajectory and is the same as the motion direction of the target motion trajectory, and the normal direction is perpendicular to the tangential direction.

[0090] In a preferred embodiment of the present application, the auxiliary force field in step S2 comprises:

[0091] Step S2.1, obtaining the treatment site position and the correction mode corresponding to the treatment site position;

[0092] Step S2.2: Obtain tangential motion capability, normal motion capability, and normal motion trend;

[0093] Step S2.3: Determine the normal auxiliary force field based on the normal motion trend and normal motion capability;

[0094] Step S2.4: Determine the tangential auxiliary force field based on the tangential motion capability;

[0095] Step S2.5: Correct the actual motion trajectory of the treatment site according to the normal auxiliary force field and the tangential auxiliary force field.

[0096] Preferably, step S2.1 includes: dividing the area surrounding the target motion trajectory into a first control area, a second control area, and a third control area according to the target motion trajectory;

[0097] like Figure 3 As shown, when 0≤d≤R s If the treatment site is located in the first control area, the trajectory of the treatment site is corrected based on the first correction mode; when R s ≤d≤R m If the treatment site is located in the second control area, the trajectory of the treatment site is corrected based on the second correction mode; when d>R m If the treatment site is located in the third control area, the trajectory of the treatment site is corrected based on the third correction mode; d represents the distance between the treatment site and the target motion trajectory, R s R represents the boundary between the first control region and the second control region. m R represents the boundary between the second and third control regions. m >R s The target's trajectory is R, P. S P represents the starting point of the trajectory. E The endpoint of the trajectory is represented by R. S .

[0098] To allow users to operate within acceptable error limits and maintain a degree of freedom, the first control area is a tolerance zone. Within this area, the treatment site's position is not corrected, and the subject's posture is not interfered with in the first correction mode. When the treatment site enters the second control area, it indicates a deviation in the subject's position or posture. In this case, a certain auxiliary force needs to be applied to the treatment site, i.e., the trajectory of the treatment site is corrected based on the second correction mode. If the treatment site enters the third control area, it indicates a significant deviation in the subject's position or posture. In this case, a larger auxiliary force needs to be applied to quickly guide the subject back to the target motion trajectory.

[0099] Specifically, such as Figure 4As shown, the distance d between the treatment site and the target trajectory is the Euclidean distance between the two:

[0100] Where, the Euclidean distance between the current point and the nearest point should be greater than the radius of curvature of the nearest point on the spatial trajectory, i.e.:

[0101] The correction mode is: F an = F an1 + F D ,

[0102]

[0103] F at = K at F atmax t,

[0104]

[0105] Where, a is the curvature of the point with the minimum Euclidean distance on the target trajectory, U is the set of spatial points reachable by the end of the robot arm, F an represents the normal auxiliary force, P N (x N ,y N ,z N ) represents the target trajectory, P act (x a ,y a ,z a ) represents the position of the treatment site, F an1 ∈ R 3×1 and F D ∈ R 3×1 respectively represent the stiffness term and the damping term of the normal auxiliary force, where the damping term F D is to ensure that the normal auxiliary force decays at a damping coefficient K D ∈ R 3×3 to make the system stable. The normal auxiliary force field in the second control region is K an , and the normal auxiliary force field in the third control region increases at a speed of a·K an as the deviation increases, where a is a change coefficient, n∈ R 3×1 represents the direction of the normal force, F atmax ∈ R 3×1 is the maximum tangential auxiliary force, K at is the tangential auxiliary coefficient, t∈ R 3 ×1 represents the direction of the tangential force, which is always the same as the movement direction of the target trajectory.

[0106] In one specific embodiment of the present application, the step S2.3 comprises:

[0107] Normal auxiliary force field

[0108] NCI = NMT · ER,

[0109]

[0110] β represents a normal motion trend coefficient, used to assist NCI to promote the subject to actively participate in motion, NCI represents the trajectory following ability of the treatment site, ER represents a weighted average error index, NMT represents a normal motion trend, K an_max and K an_min are the upper and lower limits of the normal stiffness coefficient, e p represents an error, KW i represents a time weight coefficient, KW max and KW min represent a maximum weight coefficient and a minimum weight coefficient, the maximum weight coefficient is usually taken as 1. i represents the number of time frames at the current time window, T all is the number of frames of the time window, m·ΔT represents a moving time window size, and ΔT is a control period of the robot controller.

[0111] wherein β is also realized in a moving weighted mean filtering manner, and the time window thereof is n·ΔT. When the subject tends to move along the trajectory, the coefficient rapidly decreases, resulting in that the strength of the normal auxiliary force field also rapidly decreases; when the subject moves away from the trajectory, β is 1, and the strength of the normal auxiliary force field only slowly changes according to NCI.

[0112] In one specific embodiment of the present application, the normal motion trend evaluation comprises:

[0113] The normal motion trend is calculated based on a moving weighted mean filtering method:

[0114]

[0115] is the motion direction of the target motion trajectory reference point corresponding to the current position, represents an interactive force of the treatment site, F ha ∈R 3×1 is the force F h decomposed into the normal direction, F ht ∈R 3×1 is the force F h decomposed into the tangential direction, F ha_max is the maximum normal force output by the treatment site, used for normalization processing.

[0116] When the interaction force tends to the target trajectory, the NMT result is less than 1, and the normal force field gain is reduced; when the interaction force deviates from the trajectory, the NMT result is greater than 1, and the normal force field gain is increased.

[0117] In one specific embodiment of the application, in order to guide the treatment site to move towards the target position, a tangential auxiliary force field is also needed in the tangential direction, and the size of the tangential auxiliary force can be adaptively changed according to the motion intention of the user. The tangential auxiliary force field is determined according to the tangential motion ability, when the tangential interaction force is large, it represents that the tangential motion ability is good, at this time, the auxiliary force can be provided as little as possible or not; when the subject has no motion ability, the robot needs to provide the tangential auxiliary force to guide the user to move.

[0118] Among them, the tangential motion trend of the user needs to be considered, and it is analyzed whether the subject has a trend of actively increasing the tangential output force or a trend of actively reducing the tangential output force. Under the same tangential interaction force, for the trend of actively increasing, the control system should provide smaller auxiliary force, and for the trend of actively reducing, the control system should provide larger auxiliary force.

[0119] The tangential motion ability evaluation includes that the tangential performance index is obtained by comprehensively processing the value of the tangential interaction force of the subject along the target trajectory and the change rate thereof based on the moving weighted mean filtering method:

[0120]

[0121] K at = g·TPI,

[0122]

[0123] TP and TPI respectively represent the tangential interaction force and the transformation rate of the treatment site along the target motion trajectory, dF ht_max represents the upper limit of the tangential interaction force transformation rate, used for normalization processing. K at represents the tangential auxiliary force field coefficient, and g represents the tangential motion trend coefficient.

[0124] The adaptive coefficient g is reduced when the subject increases the tangential interaction force, so that the auxiliary force is rapidly reduced; when the subject reduces the tangential interaction force, the coefficient g is 1, and the tangential auxiliary force is gradually increased according to TPI.

[0125] In summary, the application has the following effects:

[0126] The application provides a robot control method based on trend awareness, which can analyze the current motion trend of a subject, and applies adaptive auxiliary force to the subject according to the position of the treatment part of the subject and the motion trend to correct the position of the subject, thereby improving the treatment accuracy and being effectively applied to unstable environments; in addition, the compliant compliance control is introduced, so that the medical robot can safely and effectively perform safe physical interaction with the subject and the doctor.

[0127] Based on the present application, those skilled in the art will appreciate that one aspect described herein can be implemented independently of any other aspects and that some aspects can be implemented independently of one another. For example, any of the number of devices and / or methods described herein can be used with any other aspect described herein. Further, some aspects described herein can be implemented as a method, apparatus, or article of manufacture using, for example, any number of the aspects described herein. The embodiments described herein are to be considered in a descriptive sense only and not for purposes of limitation.

[0128] Those skilled in the art know that, in addition to implementing the system provided by the present application and each device, module and unit thereof in a pure computer readable program code manner, the system provided by the present application and each device, module and unit thereof can also be implemented in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps to achieve the same functions. Therefore, the system provided by the present application and each device, module and unit thereof can be considered as a hardware component, and the devices, modules and units included therein for achieving various functions can also be considered as structures within the hardware component; the devices, modules and units for achieving various functions can also be considered as both software modules for implementing methods and structures within hardware components.

[0129] It should be noted that the above embodiments can be freely combined as needed. The above only describes the preferred embodiments of the present application, and it should be pointed out that, for those skilled in the art, without departing from the principles of the present application, a number of improvements and refinements can be made, and these improvements and refinements should be considered as the protection scope of the present application.

[0130] All the documents mentioned in the present application are cited as references in the present application, just as each document is cited as a reference. In addition, it should be understood that those skilled in the art can make various modifications or modifications to the present application after reading the above description of the present application, and these equivalent forms also fall within the scope of the claims attached to the present application.

Claims

1. A trend-aware robot control method, the robot comprising at least a plurality of robotic arms, each robotic arm having a therapeutic instrument at its end cap for treating at least one treatment site of a subject, characterized in that, Includes the following steps: Step S1: Obtain the target motion trajectory of at least one robotic arm end and obtain the actual trend parameters of the treatment site. The target motion trajectory of the at least one robotic arm end is the same as the target motion trajectory of the treatment site corresponding to the robotic arm end. Step S2: Based on the target motion trajectory and the actual trend parameters, an auxiliary force field is applied, and the robot provides auxiliary control for the treatment area of ​​the subject based on the auxiliary force field to correct the actual motion trajectory of the treatment area; Step S3: Instruct at least one of the robotic arms to move according to the target motion trajectory based on admittance control; The actual trend parameters of the treatment site include at least: treatment site location, tangential motion capability, normal motion capability, and normal motion trend; the auxiliary force field includes a normal auxiliary force field and a tangential auxiliary force field. Wherein, the tangential direction represents the direction of motion that is tangential to the current position of the target trajectory and is the same as the direction of motion of the target trajectory, and the normal direction is perpendicular to the tangential direction; The auxiliary force field mentioned in step S2 includes: Step S2.1: Obtain the location of the treatment site and the corresponding correction mode; Step S2.2: Obtain tangential motion capability, normal motion capability, and normal motion trend; Step S2.3: Determine the normal auxiliary force field based on the normal motion trend and normal motion capability; Step S2.4: Determine the tangential auxiliary force field based on the tangential motion capability; Step S2.5: Correct the actual motion trajectory of the treatment site according to the normal auxiliary force field and the tangential auxiliary force field; Step S2.1 includes: dividing the area surrounding the target motion trajectory into a first control area, a second control area, and a third control area according to the target motion trajectory; in: when 0≤d≤R s If the treatment site is located in the first control area, the trajectory of the treatment site is corrected based on the first correction mode; when R s ≤d≤R m If the treatment site is located in the second control area, the trajectory of the treatment site is corrected based on the second correction mode. when d>R m If the treatment site is located in the third control area, the trajectory of the treatment site is corrected based on the third correction mode. d Indicates the distance between the treatment site and the target movement trajectory. R s This indicates the boundary between the first control area and the second control area. R m This indicates the boundary between the second and third control areas. R m ＞R s .

2. The trend-aware robot control method according to claim 1, characterized in that, In step S3, the admittance control includes: ， ， ， ， in, This is a combination of the interaction forces and torques between the treatment site and the end effector of the robotic arm. It is a combination of interactive force and interactive torque. and These represent the actual required pose and the desired pose of the robotic arm's end effector, respectively. M is the mass matrix, D is the damping matrix, and K is the stiffness matrix. x,y,z and rx, ry,rz These are the position of the robotic arm's end effector and the Euler angle, respectively. This represents the actual joint angular velocity. Let Jacobian matrix be the value of the robotic arm. The joint angular velocity is the force input to the robot. This refers to the angular velocity of the robotic arm's end effector that needs to be controlled.

3. The trend-aware robot control method according to claim 2, characterized in that, The admittance control also includes: ， ， ， ， ， in, and These are the normal auxiliary force and the tangential auxiliary force, respectively. and These are interaction force and interaction torque, respectively. The resultant force of the auxiliary force and the interaction force. and These are the mass matrix and damping matrix of the position admittance controller, respectively; The linear velocity of the robotic arm's end effector that the robot needs to control; and These are the stiffness matrix and damping matrix of the attitude admittance controller, respectively. The actual Euler angles at the end of the robotic arm. Indicates the target Euler angle.

4. The trend-aware robot control method according to claim 3, characterized in that, The distance between the treatment site and the target trajectory is the Euclidean distance between them: Among them, the following conditions need to be met. ; The correction mode is: , ， ， ， ， in, α Let be the curvature of the point with the minimum Euclidean distance on the target trajectory, and let U be the set of spatial points reachable by the robotic arm's end effector. F an Indicates the normal auxiliary force. Indicates the trajectory of the target movement. Indicates the location of the treatment site. and Let these represent the stiffness and damping terms of the normal auxiliary force, respectively, where the damping term... This is to ensure that the normal auxiliary force is in line with the damping coefficient. To attenuate, the normal auxiliary force field in the second control region is... In the third control region, the normal auxiliary force field increases with the deviation. The speed increases, a The coefficient of variation, Indicates the direction of the normal force. For maximum tangential auxiliary force, For tangential auxiliary coefficients, Indicates the direction of the tangential force.

5. The trend-aware robot control method according to claim 4, characterized in that, Step S2.3 includes: Normal auxiliary force field , ， ， ， ， ， ， β Indicates the normal motion trend coefficient. NCI This indicates the ability to track the trajectory of the treatment area. ER This represents the weighted average error index. NMT Indicates the tendency of normal motion. and These are the upper and lower limits of the normal stiffness coefficient. Indicates error. Indicates the time weighting coefficient. and This represents the maximum and minimum weight coefficients. i Indicates the current time frame number within the current time window. The number of frames in the time window. Indicates the size of the moving time window. This refers to the control cycle of the robot controller.

6. The trend-aware robot control method according to claim 5, characterized in that, The assessment of the normal motion trend includes: Normal movement trend , ， ， The direction of motion of the reference point for the target's trajectory at the current position. Indicates the interaction force at the treatment site. Interaction force at the treatment site The force decomposed into the normal direction, For interactive power Decomposed into tangential forces, The maximum normal force output to the treatment site.

7. The trend-aware robot control method according to claim 6, characterized in that, The tangential motion capability assessment includes: ， ， ， ， ， TP and TPI represent the tangential interaction force and transformation rate at the treatment site along the target motion trajectory, respectively. This represents the upper limit of the rate of change of tangential interaction force. denoted by , where g represents the tangential auxiliary force field coefficient and g represents the tangential motion tendency coefficient.

Images