Acupuncture point tracking control method and system based on visual prediction path integral

Abstract

The invention discloses an acupoint tracking control method and system based on visual prediction path integration, and relates to the technical field of robot control, and the method comprises the steps: obtaining the position and normal vector of an acupoint, and judging whether the acupoint is in the field of view of a hand-eye camera or not according to the position of the acupoint; establishing a first acupoint tracking model based on position visual servo according to the position and the normal vector; establishing a second acupoint tracking model based on mixed visual servo according to the first acupoint tracking model; when the acupoints are out of the visual field range of the hand-eye camera, acupoint tracking control is achieved in combination with a first acupoint tracking model and visual prediction path integration; and when the acupuncture points are in the view field of the hand-eye camera, realizing acupuncture point tracking control by combining the second acupuncture point tracking model and the visual prediction path integral. The acupoints are positioned through the positions and the normal vectors, acupoint tracking control is achieved according to the position classification constraint of the acupoints and the acupoint tracking model of the corresponding position, and the accuracy of acupoint tracking control is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and in particular to a point tracking control method and system based on visual prediction path integral. BACKGROUND

[0002] Traditional Chinese medicine robots, as a product of the integration of traditional Chinese medicine and modern technology, have great research significance. They can accurately perform Chinese medicine diagnosis and treatment operations, improve the stability and repeatability of treatment effects, reduce the workload of Chinese medicine practitioners, promote the standardization and internationalization of Chinese medicine technology, and benefit more people with Chinese medicine treatment. Point tracking control method is one of the key technologies of traditional Chinese medicine robots, which is crucial to improve the accuracy of traditional Chinese medicine treatment such as acupuncture. Accurate tracking of points can ensure accurate application of treatment effects, enhance treatment effects, and reduce the risk of misoperation. However, the field still faces many challenges. Existing visual servo control methods have their own limitations, image-based models have insufficient local stability, and position-based models require high system calibration accuracy and are prone to target loss due to excessive movement. However, in practical applications, traditional Chinese medicine robots also face physical problems such as joint angle / velocity limitations, and classic visual servo methods are difficult to effectively cope with. SUMMARY

[0003] The main purpose of the embodiments of the present application is to provide a point tracking control method and system based on visual prediction path integral to improve the accuracy of point tracking control of robots.

[0004] To achieve the above-mentioned purpose, one aspect of the embodiments of the present application provides a point tracking control method based on visual prediction path integral, which comprises the following steps:

[0005] Obtaining the position and normal vector of the point, and determining whether the point is within the field of view of the hand-eye camera according to the position of the point;

[0006] Establishing a first point tracking model based on position visual servo according to the position and the normal vector;

[0007] Establishing a second point tracking model based on hybrid visual servo according to the first point tracking model;

[0008] When the point is outside the field of view of the hand-eye camera, combining the first point tracking model and visual prediction path integral to realize point tracking control;

[0009] When the point is within the field of view of the hand-eye camera, combining the second point tracking model and visual prediction path integral to realize point tracking control.

[0010] In some embodiments, the position and normal vector of the point are obtained by the following steps:

[0011] defining the position and the normal vector of the acupoint with respect to the coordinate system {hec} of the hand-eye camera hec p ap = hec x ap , hec y ap , hec z ap ] T and hec n ap .

[0012] In some embodiments, the establishing a first acupoint tracking model based on position visual servoing according to the position and the normal vector comprises the following steps:

[0013] According to the unit vector e1 = [1, 0, 0] T , the rotation matrix of the acupoint with respect to {hec} hec R ap is modeled as:

[0014] hec R ap = hec n ap ×e1× hec n ap hec n ap ×e1 hec n ap ;

[0015] Define an R hec and an p hec represent the rotation matrix and the translation vector of {hec} with respect to the coordinate system {an} of the end effector of the robot arm respectively, an R hec and an p hec The homogeneous transformation matrix composed of an T hec = ( e T an ) -1 ( e T hec ); and the rotation matrix an R ap and the position vector an p ap of the acupoint with respect to {an} are represented as:

[0016] an R ap = an R hechec R ap ;

[0017] an p ap = an R hec hec p ap + an p hec ;

[0018] Define α * and d * Denote the desired angle and initial distance respectively, then the desired rotation matrix of the acupoint relative to {an} and position vector Respectively expressed as:

[0019]

[0020] Among them, R X (α) is the rotation matrix of α around the X axis; for The third column vector of ;

[0021] Determine the desired rotation matrix of the acupoint relative to {hec} through coordinate system transformation and position vector Respectively expressed as:

[0022]

[0023] definition hec V hec Represents the generalized velocity of the hand-eye camera relative to {hec}, and then the visual feature of acupoint tracking is determined as Among them, k and φ represent The corresponding rotation axis and rotation angle; define the desired visual feature as

[0024] Then the first acupoint tracking model based on position visual servoing is expressed as:

[0025]

[0026] Among them, J track is the Jacobian matrix of acupoint tracking; x^ is the antisymmetric matrix of vector x;

[0027] sinc(φ) is the sine function,

[0028] In some embodiments, the step of establishing a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model comprises the following steps:

[0029] Definitions ap =[u ap ,v ap ] T and Represent the measured value and expected value of the acupoint pixel coordinates, Visual features representing acupoint tracking;

[0030] Define the desired visual features as

[0031] Then, the second acupoint tracking model based on hybrid visual servoing is established according to the first acupoint tracking model:

[0032]

[0033] Among them, (f u ,f v ,u0,v0) are the internal parameters of the hand-eye camera;

[0034] The generalized velocity conversion model between the hand-eye camera and the end of the robotic arm is expressed as:

[0035]

[0036] Among them, e t hec The antisymmetric matrix of ;

[0037] Then, the expression of the second acupoint tracking model is converted into:

[0038]

[0039] The discrete model of the second acupoint tracking model after the conversion expression is determined according to the Newton-Euler approximation method is expressed as:

[0040]

[0041] Wherein, Δt is the time interval of acupoint tracking control.

[0042] In some embodiments, the method of combining the second acupoint tracking model with the visual prediction path integral to implement acupoint tracking control includes the following steps:

[0043] Sampling an initial joint angular velocity sequence of the robotic arm, and then predicting a first joint angular velocity sequence of the acupoint tracking trajectory according to the discrete model of the second acupoint tracking model;

[0044] Define the total cost function;

[0045] converting the first joint angular velocity sequence into a second joint angular velocity sequence that satisfies the total cost function constraint according to the visual prediction path integral;

[0046] The robotic arm is controlled to track the acupuncture points according to the second joint angular velocity sequence.

[0047] In some embodiments, the sampling of the initial joint angular velocity sequence of the robotic arm comprises the following steps:

[0048] The initial joint angular velocity sequence of the robotic arm is sampled as follows:

[0049]

[0050] The method of predicting a first joint angular velocity sequence of an acupoint tracking trajectory based on the discrete model of the second acupoint tracking model comprises the following steps:

[0051] Furthermore, the first joint angular velocity sequence of the acupoint tracking trajectory is predicted according to the discrete model of the second acupoint tracking model as follows:

[0052]

[0053] in, is the sampled joint angular velocity deviation, Obeys a Gaussian distribution with zero mean.

[0054] In some embodiments, defining the total cost function comprises the following steps:

[0055] definition and Represent the minimum and maximum values ​​of the joint angular velocity, respectively, and The lower and upper bounds of the constraints are expressed as and Thus, the joint angular velocity constraint function is determined as:

[0056]

[0057] Then the visual feature sequence of the kth acupoint tracking trajectory is determined as:

[0058]

[0059] Define the failure cost of the kth acupoint tracking trajectory as c fail,k ;

[0060]

[0061] The acupoint tracking cost function at time t is defined as express and the expected visual feature h * The quadratic error between

[0062]

[0063] Among them, Q track is the weight matrix of the acupoint tracking cost function;

[0064] The acupoint tracking control cost at time t is defined as Expressed as and A quadratic function of

[0065]

[0066] Among them, Q ctrl is the weight matrix of the acupoint tracking control cost; ν is the exploration noise;

[0067] Define the visibility constraint cost at time t as so that the acupuncture points are within the field of view of the hand-eye camera;

[0068]

[0069] Then the total cost function for obtaining the kth acupoint tracking trajectory is determined to be c k ;

[0070]

[0071] The step of converting the first joint angular velocity sequence into a second joint angular velocity sequence that satisfies the total cost function constraint according to the visual prediction path integral comprises the following steps:

[0072] The first joint angular velocity sequence is converted into the second joint angular velocity sequence satisfying the total cost function constraint according to the visual prediction path integral:

[0073]

[0074] Among them, λ track is the inverse temperature, λ track The selection level used to determine the weighted average; c min is the minimum cost of the sampled acupoint tracking trajectory,

[0075] To achieve the above objectives, another aspect of the present application provides an acupoint tracking control system based on visual prediction path integration, the system comprising:

[0076] An acupoint positioning unit is used to obtain the position and normal vector of the acupoint and determine whether the acupoint is within the field of view of the hand-eye camera based on the position of the acupoint;

[0077] A first model building unit is configured to build a first acupoint tracking model based on position visual servoing according to the position and the normal vector;

[0078] A second model building unit is configured to build a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model;

[0079] a first acupoint tracking control unit, configured to implement acupoint tracking control by combining the first acupoint tracking model and visual prediction path integration when the acupoint is outside the field of view of the hand-eye camera;

[0080] The second acupoint tracking control unit is used to implement acupoint tracking control by combining the second acupoint tracking model and visual prediction path integration when the acupoint is within the field of view of the hand-eye camera.

[0081] To achieve the above-mentioned purpose, another aspect of an embodiment of the present application provides an electronic device, which includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the above-mentioned method when executing the computer program.

[0082] To achieve the above-mentioned purpose, another aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program implements the above-mentioned method when executed by a processor.

[0083] The embodiments of the present application include at least the following beneficial effects:

[0084] This application can obtain the position and normal vector of an acupoint, and determine whether the acupoint is within the field of view of a hand-eye camera based on the position of the acupoint; establish a first acupoint tracking model based on position visual servoing based on the position and normal vector; establish a second acupoint tracking model based on hybrid visual servoing based on the first acupoint tracking model; when the acupoint is outside the field of view of the hand-eye camera, acupoint tracking control is achieved by combining the first acupoint tracking model and visual prediction path integration; when the acupoint is within the field of view of the hand-eye camera, acupoint tracking control is achieved by combining the second acupoint tracking model and visual prediction path integration. This application locates acupoints by acupoint position and normal vector, implements acupoint tracking control based on classification constraints according to the actual position of the acupoint and utilizes the acupoint tracking model of the corresponding position, thereby improving the accuracy of acupoint tracking control. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0086] Figure 1 A flow chart of an acupoint tracking control method based on visual prediction path integration provided in an embodiment of the present application;

[0087] Figure 2 This is an example flow chart of an acupoint tracking control method based on visual prediction path integration provided in an embodiment of the present application;

[0088] Figure 3 An example diagram of possible locations of various units in different wire meshes provided in the embodiments of the present application;

[0089] Figure 4 An example diagram of optimized space in wire meshes of different densities provided in an embodiment of the present application;

[0090] Figure 5 An example diagram of optimized space in wire meshes of different densities provided in an embodiment of the present application;

[0091] Figure 6 An example diagram of optimized space in wire meshes of different densities provided in an embodiment of the present application;

[0092] Figure 7 A schematic diagram of the structure of an acupoint tracking control system based on visual prediction path integration provided in an embodiment of the present application;

[0093] Figure 8 A schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0094] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the embodiments of the present application. They are merely examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the appended claims.

[0095] It will be understood that the terms "first", "second", etc. used in this application may be used herein to describe various concepts, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another. For example, without departing from the scope of the embodiments of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the words "if" and "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0096] The terms "at least one", "plurality", "each", "any", etc. used in this application include "at least one", "two" or more, "plurality" or "each", "any" or "any one", "each" or "any one" as used herein.

[0097] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which this application pertains. The terms used herein are for the purpose of describing the embodiments of this application only and are not intended to limit this application.

[0098] Some parameters are described as follows:

[0099] {0} and {e} represent the base coordinate system and end coordinate system of the manipulator, respectively;

[0100] {gc} represents the global camera coordinate system;

[0101] {hec} represents the coordinate system of the hand-eye camera;

[0102] {an} represents the coordinate system of the end effector of the robot arm;

[0103] {ap} represents the coordinate system of the acupoint.

[0104] Reference Figure 1 The embodiment of the present application provides an acupoint tracking control method based on visual prediction path integration. The method may include but is not limited to S100 to S140, as follows:

[0105] S100: Obtain the position and normal vector of the acupoint, and determine whether the acupoint is within the field of view of the hand-eye camera based on the position of the acupoint;

[0106] S110: establishing a first acupoint tracking model based on position visual servoing according to the position and the normal vector;

[0107] S120: Establishing a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model;

[0108] S130: When the acupoint is outside the field of view of the hand-eye camera, implementing acupoint tracking control by combining the first acupoint tracking model and visual prediction path integration;

[0109] S140: When the acupoint is within the field of view of the hand-eye camera, acupoint tracking control is implemented by combining the second acupoint tracking model and visual prediction path integration.

[0110] Optionally, obtaining the position and normal vector of the acupoint comprises the following steps:

[0111] The position and normal vector of the acupoint relative to the hand-eye camera coordinate system {hec} are defined as hec p ap =[ hec x ap , hec y ap , hec z ap ] T and hec n ap .

[0112] Optionally, establishing a first acupoint tracking model based on position visual servoing according to the position and the normal vector comprises the following steps:

[0113] According to the unit vector e1=[1,0,0] T , the rotation matrix of the acupoint relative to {hec} hec R ap Modeled as:

[0114] hec R ap =[ hec n ap ×e1× hec n ap hec n ap ×e1 hec n ap ];

[0115] definition an R hec and an p hec They represent the rotation matrix and translation vector of {hec} relative to the coordinate system {an} of the end effector of the robot arm, an R hec and an p hec The homogeneous transformation matrix composed of an T hec =( e T an )-1 ( e T hec ); and then the rotation matrix of the acupoint relative to {an} an R ap and position vector an p ap Respectively expressed as:

[0116] an R ap = an R hec hec R ap ;

[0117] an p ap = an R hec hec p ap + an p hec ;

[0118] Define α * and d * Denote the desired angle and initial distance respectively, then the desired rotation matrix of the acupoint relative to {an} and position vector Respectively expressed as:

[0119]

[0120] Among them, R X (α) is the rotation matrix of α around the X axis; for The third column vector of ;

[0121] Determine the desired rotation matrix of the acupoint relative to {hec} through coordinate system transformation and position vector Respectively expressed as:

[0122]

[0123] definition hec V hec Represents the generalized velocity of the hand-eye camera relative to {hec}, and then the visual feature of acupoint tracking is determined as Among them, k and φ represent The corresponding rotation axis and rotation angle; define the desired visual feature as

[0124] Then the first acupoint tracking model based on position visual servoing is expressed as:

[0125]

[0126] Among them, J track is the Jacobian matrix of acupoint tracking; x ^ is the antisymmetric matrix of vector x;

[0127] sinc(φ) is the sine function,

[0128] Optionally, establishing a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model comprises the following steps:

[0129] Definitions ap =[u ap ,v ap ] T and Represent the measured value and expected value of the acupoint pixel coordinates, Visual features representing acupoint tracking;

[0130] Define the desired visual features as

[0131] Then, the second acupoint tracking model based on hybrid visual servoing is established according to the first acupoint tracking model:

[0132]

[0133]

[0134] Among them, (f u ,f v ,u0,v0) are the internal parameters of the hand-eye camera;

[0135] The generalized velocity conversion model between the hand-eye camera and the end of the robotic arm is expressed as:

[0136]

[0137] Among them, e t hec The antisymmetric matrix of ;

[0138] Then, the expression of the second acupoint tracking model is converted into:

[0139]

[0140] The discrete model of the second acupoint tracking model after the conversion expression is determined according to the Newton-Euler approximation method is expressed as:

[0141]

[0142] Wherein, Δt is the time interval of acupoint tracking control.

[0143] Optionally, the combining of the second acupoint tracking model and visual prediction path integration to realize acupoint tracking control comprises the following steps:

[0144] Sampling an initial joint angular velocity sequence of the robotic arm, and then predicting a first joint angular velocity sequence of the acupoint tracking trajectory according to the discrete model of the second acupoint tracking model;

[0145] Define the total cost function;

[0146] converting the first joint angular velocity sequence into a second joint angular velocity sequence that satisfies the total cost function constraint according to the visual prediction path integral;

[0147] The robotic arm is controlled to track the acupuncture points according to the second joint angular velocity sequence.

[0148] Optionally, the sampling of the initial joint angular velocity sequence of the robotic arm comprises the following steps:

[0149] The initial joint angular velocity sequence of the robotic arm is sampled as follows:

[0150]

[0151] The method of predicting a first joint angular velocity sequence of an acupoint tracking trajectory based on the discrete model of the second acupoint tracking model comprises the following steps:

[0152] Furthermore, the first joint angular velocity sequence of the acupoint tracking trajectory is predicted according to the discrete model of the second acupoint tracking model as follows:

[0153]

[0154] in, is the sampled joint angular velocity deviation, Obeys a Gaussian distribution with zero mean.

[0155] Optionally, defining the total cost function comprises the following steps:

[0156] definition and Represent the minimum and maximum values ​​of the joint angular velocity, respectively, and The lower and upper bounds of the constraints are expressed as and Thus, the joint angular velocity constraint function is determined as:

[0157]

[0158] Further, the visual feature sequence of the kth acupoint tracking trajectory is determined as:

[0159]

[0160] The failure cost of the kth acupoint tracking trajectory is defined as c fail,k ;

[0161]

[0162] The acupoint tracking cost function at time t is defined as The quadratic form error between and the expected visual feature h * is represented as:

[0163]

[0164] Wherein, Q track is the weight matrix of the acupoint tracking cost function;

[0165] The acupoint tracking control cost at time t is defined as The quadratic form function of and is represented as:

[0166]

[0167] Wherein, Q ctrl is the weight matrix of the acupoint tracking control cost; and v is the exploration noise;

[0168] The visibility constraint cost at time t is defined as So that the acupoint is within the field of view of the hand-eye camera;

[0169]

[0170] Further, the total cost function of the kth acupoint tracking trajectory is determined as c k ;

[0171]

[0172] The first joint angular velocity sequence is converted into a second joint angular velocity sequence satisfying the constraint of the total cost function according to the visual prediction path integral, including the following steps:

[0173] The first joint angular velocity sequence is converted into a second joint angular velocity sequence satisfying the constraint of the total cost function according to the visual prediction path integral, which is:

[0174]

[0175] Among them, λ track is the inverse temperature, λ track The selection level used to determine the weighted average; c min is the minimum cost of the sampled acupoint tracking trajectory,

[0176] Next, the solution of the embodiment of the present application will be introduced and explained in detail with reference to specific application examples.

[0177] In order to solve the problems existing in the existing technology, this embodiment introduces a model predictive control method to achieve trajectory optimization of acupoint tracking while meeting constraints under various conditions, which can provide strong support for the development of traditional Chinese medicine robots.

[0178] Reference Figure 2 The technical solution of this embodiment may include the following steps:

[0179] Get the three-dimensional position and two-dimensional normal vector of the acupoint;

[0180] Establish an acupoint tracking model based on position visual servoing and implement acupoint tracking control by combining visual prediction path integration;

[0181] An acupoint tracking model based on hybrid visual servoing is established, and the robot's posture fine-tuning is achieved by combining visual prediction path integration.

[0182] Next, each technical solution of this embodiment is described in detail.

[0183] 1. Acupoint tracking model based on visual servoing.

[0184] When the acupoints are beyond the field of view of the hand-eye camera, the acupoint tracking model based on position visual servoing is used to achieve acupoint tracking. hec p ap =[ hec x ap , hec y ap , hec z ap ] T and hec n ap They represent the three-dimensional position and normal vector of the acupuncture point relative to {hec}. Therefore, according to the unit vector e1=[1,0,0] T , the rotation matrix of the acupoint relative to {hec} hec R ap Can be modeled as:

[0185] hec R ap =[ hec nap ×e1× hec n ap hec n ap ×e1 hec n ap ] (1)

[0186] Assume an R hec and an p hec represent the rotation matrix and translation vector of {hec} with respect to {an}, respectively. The homogeneous transformation matrix composed of them can be expressed as an T hec = ( e T an ) -1 ( e T hec ). Therefore, the rotation matrix an R ap and the position vector an p ap of the acupoint with respect to {an} can be expressed as:

[0187] an R ap = R an R hec R hec R ap (2)

[0188] an p ap = R an R hec p hec p ap + an p hec (3)

[0189] Assume α * and d * represent the desired angle and initial distance, respectively. The desired rotation matrix and the position vector of the acupoint with respect to {an} can be expressed as:

[0190]

[0191] where R X (α) is the rotation matrix around the X axis by α;

[0192] is the third column vector of R .

[0193] Through coordinate system transformation, the desired rotation matrix and position vector can be respectively expressed as:

[0194]

[0195] Assume that hecVhec represents the generalized velocity of the hand-eye camera relative to {hec}. According to equations (4-1)~(4-7), the visual feature of the acupoint tracking can be expressed as where k and represent the rotation axis and rotation angle corresponding to respectively, and in particular, the desired visual feature can be expressed as . Therefore, the acupoint tracking model can be expressed as:

[0196]

[0197] where J track is the Jacobian matrix of the acupoint tracking;

[0198] x ^ is the skew-symmetric matrix of vector x;

[0199] sinc(φ) is the sinc function,

[0200] When the acupoint is within the field of view of the hand-eye camera, the acupoint tracking is achieved by using the hybrid visual servoing model. In the image, the position tracking of the acupoint can be simplified as the tracking of a single feature point. In addition, in the Cartesian space, the pose tracking is achieved based on the acupoint normal vector. Assume that s ap = [u ap ,v ap ] T and respectively represent the measured value and the desired value of the acupoint pixel coordinates, the visual feature of the acupoint tracking. In particular, the desired visual feature is Therefore, the acupoint tracking model based on the hybrid visual servoing can be expressed as:

[0201]

[0202] where (f u ,f v ,u0,v0) are the intrinsic parameters of the hand-eye camera.

[0203] In addition, the generalized velocity conversion model between the hand-eye camera and the end of the robot arm can be expressed as:

[0204]

[0205] where is the e t hecThe antisymmetric matrix of .

[0206] Combining equations (8) to (15), the acupoint tracking model of the robot can be expressed as:

[0207]

[0208] According to the Newton-Euler approximation method, the discrete model of Equation (16) can be expressed as:

[0209]

[0210] Where Δt is the time interval of acupoint tracking control.

[0211] 2. Acupoint tracking control based on visual prediction path integration.

[0212] The visual prediction path integration method samples the joint angular velocity sequence of the manipulator and then obtains K predicted trajectories with a time domain of T according to the discrete model of formula (17). Assume represents the initial joint angular velocity sequence of the acupoint tracking trajectory, then the joint angular velocity sequence of the kth (k=1,2,…,K) acupoint tracking trajectory can be expressed as:

[0213]

[0214] In the formula is the sampled joint angular velocity deviation, which obeys a zero-mean Gaussian distribution.

[0215] In addition, the sampling of acupoint tracking trajectory needs to meet the joint angle / velocity constraints of the robot arm. and represent the minimum and maximum values ​​of the joint angular velocity, respectively. Therefore, The lower and upper bounds of the constraints can be expressed as and Thus, a constraint function of joint angular velocity is designed, namely:

[0216]

[0217] Therefore, the visual feature sequence of the kth acupoint tracking trajectory can be expressed as:

[0218]

[0219] For the kth acupoint tracking trajectory, its cost function is established. First, in order to ensure the safety of acupoint tracking control, the failure cost c is defined fail,k , which is a discontinuous cost function. When acupoint tracking trajectory encounters dangerous situations such as collision, the failure cost is set to the maximum value, that is:

[0220]

[0221] Secondly, in order to ensure the accuracy of the acupoint tracking control, the cost function at time t is defined as It is expressed as and the quadratic form error between the expected visual feature h * and the actual visual feature h

[0222]

[0223] where Q track is the weight matrix of the acupoint tracking cost function.

[0224] Finally, in order to improve the control efficiency of the acupoint tracking, the control cost at time t is defined as It is expressed as and the quadratic form of , that is:

[0225]

[0226] where Q ctrl is the weight matrix of the acupoint tracking control cost; v is the exploration noise, which determines the exploration intensity in the joint angle space.

[0227] In addition, for the hybrid visual servoing model, in order to ensure that the acupoint does not exceed the field of view of the hand-eye camera, the visibility constraint cost at time t is defined, which is a discontinuous cost function. When the acupoint exceeds the field of view, the constraint cost is set to a large value, that is:

[0228]

[0229] Therefore, the total cost function of the kth acupoint tracking trajectory can be expressed as:

[0230]

[0231] According to the path integral optimal control theory, the update model of the joint angle velocity sequence of the robot arm can be expressed as:

[0232]

[0233] where λ track wei is the inverse temperature, which determines the selection level of the weighted average; c min is the minimum cost of the sampled acupoint tracking trajectory,

[0234] 3. Simulation results.

[0235] To verify the effectiveness of the hybrid visual prediction model-based acupoint tracking controller provided by this embodiment, a classic position-based servo control method was used for comparison with this embodiment. In the first stage, the classic method is limited by preset motion control parameters, while the controller proposed in this embodiment is able to search for the optimal control strategy and quickly reach the vicinity of the target pose. In the second stage, the method proposed in this embodiment can quickly track the target acupoint on the image plane while simultaneously controlling the angle, with a stability error lower than that of the classic method.

[0236] Exemplarily, the simulation results are described with reference to the accompanying drawings.

[0237] Figure 3 This is the position tracking error curve of the first stage; Figure 4 is the posture tracking error curve of the first stage; Figure 5 is the pixel tracking error curve of the second stage; Figure 6 This is the posture tracking error curve of the second stage.

[0238] Reference Figure 7 The embodiment of the present application further provides an acupoint tracking control system based on visual prediction path integration, which can implement the above-mentioned acupoint tracking control method based on visual prediction path integration. The system includes:

[0239] An acupoint positioning unit is used to obtain the position and normal vector of the acupoint and determine whether the acupoint is within the field of view of the hand-eye camera based on the position of the acupoint;

[0240] A first model building unit is configured to build a first acupoint tracking model based on position visual servoing according to the position and the normal vector;

[0241] A second model building unit is configured to build a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model;

[0242] a first acupoint tracking control unit, configured to implement acupoint tracking control by combining the first acupoint tracking model and visual prediction path integration when the acupoint is outside the field of view of the hand-eye camera;

[0243] The second acupoint tracking control unit is used to implement acupoint tracking control by combining the second acupoint tracking model and visual prediction path integration when the acupoint is within the field of view of the hand-eye camera.

[0244] It can be understood that the contents of the above method embodiments are all applicable to the present system embodiments, the functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0245] The embodiment of the present application further provides an electronic device, which comprises a memory and a processor. The memory stores a computer program, and the processor implements the method of the embodiment of the present application when executing the computer program. The electronic device can be any intelligent terminal, such as a tablet computer or a vehicle-mounted computer.

[0246] It can be understood that the contents in the method embodiments are applicable to the device embodiments, the device embodiments specifically implement the functions of the method of the present application, and achieve the same beneficial effects as the method of the present application.

[0247] Please refer to Figure 8 , Figure 8 The hardware structure of the electronic device of another embodiment is illustrated, and the electronic device comprises:

[0248] The processor 801 can be implemented in a general-purpose CPU (Central Processing Unit), a microprocessor, an ASIC (Application Specific Integrated Circuit), or one or more integrated circuits, and is used to execute related programs to implement the technical solutions provided by the embodiments of the present application.

[0249] The memory 802 can be implemented in the form of a ROM (Read Only Memory), a static storage device, a dynamic storage device, or a RAM (Random Access Memory). The memory 802 can store an operating system and other application programs. When the technical solutions provided by the embodiments of the present application are implemented by software or firmware, the related program codes are stored in the memory 802 and are called and executed by the processor 801 to implement the method of the present application.

[0250] The input / output interface 803 is used to realize information input and output.

[0251] The communication interface 804 is used to realize the communication interaction between the device and other devices. The communication can be realized by a wired manner (for example, a USB, a network cable, etc.) or a wireless manner (for example, a mobile network, WIFI, Bluetooth, etc.).

[0252] The bus 805 is used to transmit information between various components (for example, the processor 801, the memory 802, the input / output interface 803, and the communication interface 804) of the device.

[0253] The processor 801, the memory 802, the input / output interface 803, and the communication interface 804 are connected to each other through the bus 805 to realize the communication connection between them in the device.

[0254] An embodiment of the present application further provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the method of the present application is implemented.

[0255] It can be understood that the contents of the above method embodiments are all applicable to the present storage medium embodiment, the functions specifically implemented by the present storage medium embodiment are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0256] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely arranged relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0257] The embodiments described in the embodiments of this application are intended to more clearly illustrate the technical solutions of the embodiments of this application and do not constitute a limitation on the technical solutions provided by the embodiments of this application. Those skilled in the art will appreciate that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided in the embodiments of this application are also applicable to similar technical problems.

[0258] Those skilled in the art will understand that the technical solutions shown in the figures do not constitute a limitation on the embodiments of the present application, and may include more or fewer steps than shown in the figures, or a combination of certain steps, or different steps.

[0259] The system embodiment described above is merely illustrative. The units described as separate components may or may not be physically separate, i.e., they may be located in one place or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of this embodiment.

[0260] Those skilled in the art will appreciate that all or some of the steps in the methods, systems, and functional modules / units in the devices disclosed above may be implemented as software, firmware, hardware, or appropriate combinations thereof.

[0261] The terms "first", "second", "third", "fourth", etc. (if any) in the specification of the present application and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0262] It should be understood that in this application, "at least one (item)" means one or more, and "plurality" means two or more. "And / or" is used to describe the association relationship of associated objects, indicating that three relationships may exist. For example, "A and / or B" can mean: only A exists, only B exists, and A and B exist at the same time, where A and B can be singular or plural. The character " / " generally indicates that the previous and next associated objects are in an "or" relationship. "At least one of the following items" or similar expressions refers to any combination of these items, including any combination of single items or plural items. For example, at least one of a, b or c can mean: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, c can be single or multiple.

[0263] In the several embodiments provided in this application, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are merely illustrative. For example, the division of the above units is merely a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of systems or units, which can be electrical, mechanical or other forms.

[0264] The units described above as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0265] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0266] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes multiple instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk, and other media that can store programs.

[0267] The preferred embodiments of the present invention are described above with reference to the accompanying drawings, but are not intended to limit the scope of the present invention. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and essence of the present invention should be within the scope of the present invention.

Claims

1. An acupoint tracking control method based on visual prediction path integration, characterized in that: The method comprises the following steps: Obtain the position and normal vector of the acupoint, and determine whether the acupoint is within the field of view of the hand-eye camera based on the position of the acupoint; Establishing a first acupoint tracking model based on position visual servoing according to the position and the normal vector; Establishing a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model; When the acupoint is outside the field of view of the hand-eye camera, acupoint tracking control is implemented by combining the first acupoint tracking model and visual prediction path integration; When the acupoint is within the field of view of the hand-eye camera, the acupoint tracking control is implemented by combining the second acupoint tracking model and the visual prediction path integral.

2. The acupoint tracking control method based on visual prediction path integration according to claim 1, characterized in that: The method of obtaining the position and normal vector of the acupuncture point comprises the following steps: The position and normal vector of the acupoint relative to the hand-eye camera coordinate system {hec} are defined as hec p ap =[ hec x ap , hec y ap , hec z ap ] T and hec n ap .

3. The acupoint tracking control method based on visual prediction path integration according to claim 2, characterized in that: The step of establishing a first acupoint tracking model based on position visual servoing according to the position and the normal vector comprises the following steps: According to the unit vector e1=[1,0,0] T , the rotation matrix of the acupoint relative to {hec} hec R ap Modeled as: hec R ap =[ hec n ap ×e1× hec n ap hec n ap ×e1 hec n ap ]; definition an R hec and an p hec They represent the rotation matrix and translation vector of {hec} relative to the coordinate system {an} of the end effector of the robot arm, an R hec and an p hec The homogeneous transformation matrix composed of an T hec =( e T an ) -1 ( e T hec ); and then the rotation matrix of the acupoint relative to {an} an R ap and position vector an p ap Respectively expressed as: an R ap = an R hec hec R ap ; an p ap = an R hec hec p ap + an p hec ; Define α * and d * Denote the desired angle and initial distance respectively, then the desired rotation matrix of the acupoint relative to {an} and position vector Respectively expressed as: Among them, R X (α) is the rotation matrix of α around the X axis; for The third column vector of ; Determine the desired rotation matrix of the acupoint relative to {hec} through coordinate system transformation and position vector Respectively expressed as: definition hec V hec Represents the generalized velocity of the hand-eye camera relative to {hec}, and then the visual feature of acupoint tracking is determined as Among them, k and φ represent The corresponding rotation axis and rotation angle; define the desired visual feature as Then the first acupoint tracking model based on position visual servoing is expressed as: Among them, J track is the Jacobian matrix of acupoint tracking; x^ is the antisymmetric matrix of vector x; sinc(φ) is the sine function, 4. The acupoint tracking control method based on visual prediction path integration according to claim 1, characterized in that: The step of establishing a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model comprises the following steps: Definitions ap =[u ap ,v ap ] T and Represent the measured value and expected value of the acupoint pixel coordinates, Visual features representing acupoint tracking; Define the desired visual features as Then, the second acupoint tracking model based on hybrid visual servoing is established according to the first acupoint tracking model: Among them, (f u ,f v ,u0,v0) are the internal parameters of the hand-eye camera; The generalized velocity conversion model between the hand-eye camera and the end of the robotic arm is expressed as: Among them, e t hec The antisymmetric matrix of ; Then, the expression of the second acupoint tracking model is converted into: The discrete model of the second acupoint tracking model after the conversion expression is determined according to the Newton-Euler approximation method is expressed as: Wherein, Δt is the time interval of acupoint tracking control.

5. The acupoint tracking control method based on visual prediction path integration according to claim 1, characterized in that: The method of combining the second acupoint tracking model and the visual prediction path integral to realize acupoint tracking control includes the following steps: Sampling an initial joint angular velocity sequence of the robotic arm, and then predicting a first joint angular velocity sequence of the acupoint tracking trajectory according to the discrete model of the second acupoint tracking model; Define the total cost function; converting the first joint angular velocity sequence into a second joint angular velocity sequence that satisfies the total cost function constraint according to the visual prediction path integral; The robotic arm is controlled to track the acupuncture points according to the second joint angular velocity sequence.

6. The acupoint tracking control method based on visual prediction path integration according to claim 5, characterized in that: The initial joint angular velocity sequence of the sampling robot arm includes the following steps: The initial joint angular velocity sequence of the robotic arm is sampled as follows: The method of predicting a first joint angular velocity sequence of an acupoint tracking trajectory based on the discrete model of the second acupoint tracking model comprises the following steps: Furthermore, the first joint angular velocity sequence of the acupoint tracking trajectory is predicted according to the discrete model of the second acupoint tracking model as follows: in, is the sampled joint angular velocity deviation, Obeys a Gaussian distribution with zero mean.

7. The acupoint tracking control method based on visual prediction path integration according to claim 6, characterized in that: Describing the total cost function comprises the following steps: definition and Represent the minimum and maximum values ​​of the joint angular velocity, respectively, and The lower and upper bounds of the constraints are expressed as and Thus, the joint angular velocity constraint function is determined as: Then the visual feature sequence of the kth acupoint tracking trajectory is determined as: Define the failure cost of the kth acupoint tracking trajectory as c fail,k ; The acupoint tracking cost function at time t is defined as express and the expected visual features h * The quadratic error between Among them, Q track is the weight matrix of the acupoint tracking cost function; The acupoint tracking control cost at time t is defined as Expressed as and A quadratic function of Among them, Q ctrl is the weight matrix of the acupoint tracking control cost; ν is the exploration noise; Define the visibility constraint cost at time t as so that the acupuncture points are within the field of view of the hand-eye camera; Then the total cost function for obtaining the kth acupoint tracking trajectory is determined to be c k ; The step of converting the first joint angular velocity sequence into a second joint angular velocity sequence that satisfies the total cost function constraint according to the visual prediction path integral comprises the following steps: The first joint angular velocity sequence is converted into the second joint angular velocity sequence satisfying the total cost function constraint according to the visual prediction path integral: Among them, λ track is the inverse temperature, λ track The selection level used to determine the weighted average; c min is the minimum cost of the sampled acupoint tracking trajectory, 8. An acupoint tracking control system based on visual prediction path integration, characterized in that: The system comprises: An acupoint positioning unit is used to obtain the position and normal vector of the acupoint and determine whether the acupoint is within the field of view of the hand-eye camera based on the position of the acupoint; A first model building unit is configured to build a first acupoint tracking model based on position visual servoing according to the position and the normal vector; A second model building unit is configured to build a second acupoint tracking model based on hybrid visual servoing according to the first acupoint tracking model; a first acupoint tracking control unit, configured to implement acupoint tracking control by combining the first acupoint tracking model and visual prediction path integration when the acupoint is outside the field of view of the hand-eye camera; The second acupoint tracking control unit is used to implement acupoint tracking control by combining the second acupoint tracking model and visual prediction path integration when the acupoint is within the field of view of the hand-eye camera.

9. An electronic device, characterized in that: The electronic device includes a memory and a processor, the memory stores a computer program, and the processor implements the method according to any one of claims 1 to 7 when executing the computer program.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images