Impedance control method for ultrasonic robot

Abstract

The invention discloses an impedance control method for an ultrasonic robot, and relates to the technical field of automatic control, and the method comprises the steps: carrying out the dynamic modeling of a multi-degree-of-freedom mechanical arm of the ultrasonic robot, obtaining a Lagrange-form dynamic equation of the mechanical arm in a joint space, and enabling the dynamic equation to comprise impedance constraint; human body key points are recognized, and then an ultrasonic probe of the ultrasonic robot is controlled to move to the human body key points; trajectory planning is carried out according to the human body key points, and poses of the ultrasonic probe on the multiple path points are obtained; determining a joint reference torque according to the tail end expected acting force and the pose on each path point; and controlling the ultrasonic robot according to the joint reference torque and the kinetic equation. According to the method, the dynamic modeling of the multi-degree-of-freedom mechanical arm and the joint impedance are combined for control, and efficient and accurate scanning of the complex body surface can be achieved.

Description

Technical Field

[0001] The present application relates to the technical field of automatic control, and particularly relates to an impedance control method for an ultrasonic robot. Background Art

[0002] With the continuous development of robot technology, its application in medical diagnosis has gradually increased. Especially during ultrasonic examinations, autonomous scanning is achieved through a robotic arm to assist ultrasonic physicians in making diagnoses. When scanning the patient's skin surface, the compliance of the robotic arm is very important. It is necessary to ensure the stability of the contact force to present clear ultrasonic images and at the same time ensure safety in human-robot collaboration. The research on the compliant control of ultrasonic robots is very necessary. Force compliant control can be roughly divided into two categories according to its implementation methods: 1) admittance models based on position inner-loop control, and 2) impedance models based on force inner-loop control.

[0003] Admittance control measures the end effector force by installing a force sensor, converts the end effector force error into a position error, and then performs position closed-loop control. Impedance control converts the end position error into an end operating force and then performs closed-loop control of the operating force. Impedance control has the advantages of high dynamic response and low jitter compared with admittance control and has research value. Impedance control can flexibly adjust according to changes in the environment (such as changes caused by physiological phenomena such as human breathing and movement) by real-time adjusting the relationship between the force and displacement at the end of the robot, adapting to the diversity and softness of the human body surface. Although existing research has applied impedance control strategies in the robot joint space and task space, the technical solutions for applying impedance control to multi-degree-of-freedom ultrasonic robots still have problems with insufficient accuracy. Summary of the Invention

[0004] The main purpose of the embodiments of the present application is to propose an impedance control method for an ultrasonic robot to improve the impedance control accuracy of a multi-degree-of-freedom ultrasonic robot.

[0005] To achieve the above object, on the one hand, an embodiment of the present application proposes an impedance control method for an ultrasonic robot, and the method includes the following steps:

[0006] Perform dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in the joint space; wherein, the dynamic equation includes impedance constraints;

[0007] Identify human key points, and then control the ultrasonic probe of the ultrasonic robot to move to the human key points;

[0008] Perform trajectory planning according to the human key points to obtain the poses of the ultrasonic probe at multiple path points;

[0009] Determine the joint reference torque according to the expected end force and the poses at each of the path points;

[0010] Control the ultrasonic robot according to the joint reference torque and the dynamic equation.

[0011] In some embodiments, the dynamic modeling of the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in the joint space includes the following steps:

[0012] Perform dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in the joint space as follows:

[0013]

[0014] where M, C, and G are the mass matrix, the centripetal force matrix, and the gravity matrix respectively; θ, are the joint angle, the joint angular velocity, and the joint angular acceleration respectively; τ is the joint input torque, and τ e is the joint impedance torque.

[0015] In some embodiments, the steps for determining the joint impedance torque include the following steps:

[0016] Establish the joint impedance torque in the joint space as follows:

[0017]

[0018] where Δq is the error between the desired joint angle and the actual joint angle, is the error between the desired joint angular velocity and the actual joint angular velocity, is the error between the desired joint angular acceleration and the actual joint angular acceleration; M(q)=M d .

[0019] In some embodiments, the trajectory planning according to the human key points to obtain the poses of the ultrasonic probe at multiple path points includes the following steps:

[0020] Determine the starting point and the ending point corresponding to the human key points;

[0021] Generate a plurality of equally spaced points between the starting point and the ending point;

[0022] Determine the normal vectors corresponding to each of the equally spaced points;

[0023] Calculate the path points according to the normal vectors corresponding to each of the equally spaced points;

[0024] Take the normal vector as z iAxis, calculate x based on the straight line connecting the current path point and the next path point i Axis, and then calculate y i Axis;

[0025] Based on the z i Axis, the x i Axis and the y i Axis, calculate the pose of the ultrasonic probe at the corresponding path point

[0026] In some embodiments, determining the normal vectors corresponding to each of the equidistant points includes the following steps:

[0027] Determine the local surface of the point cloud within the preset range of each equidistant point; solve the minimum eigenvalue of the covariance matrix of the local surface of the point cloud; determine the eigenvector corresponding to the minimum eigenvalue as the normal vector;

[0028] The covariance matrix is:

[0029]

[0030] Among them, K i Represents the local surface of the point cloud, m represents the number of selected points, Represents the centroid, X represents the covariance matrix;

[0031] Calculating the path point according to the normal vectors corresponding to each of the equidistant points includes the following steps:

[0032] Calculate the path point according to the normal vectors corresponding to each of the equidistant points as follows:

[0033]

[0034] Among them, ρ i Represents the path point, σ i Represents the equidistant point, n represents the normal vector, p i Represents the point cloud surface point; P represents the set of point cloud surface points;

[0035] Calculating the x i Axis based on the straight line connecting the current path point and the next path point, and then calculating the y i Axis includes the following steps:

[0036] Calculate the straight line as:

[0037]

[0038] Among them, yy i Represents the straight line of the current path point and the next path point; ρi Represents the currently described path point; ρ i+1 Represents the next described path point;

[0039] Calculate the x i axis as:

[0040]

[0041] Calculate the y i axis as:

[0042]

[0043] In some embodiments, determining the joint reference torque according to the desired end effector force and the poses at each of the path points includes the following steps:

[0044] Determine the joint reference torque according to the desired end effector force and the poses at each of the path points as follows:

[0045]

[0046] where τ r represents the joint reference torque; represents the Jacobian matrix of the robotic arm in the ultrasonic probe coordinate system U; represents the deviation between the reference pose and the current pose of the ultrasonic robot in the ultrasonic probe coordinate system U; and ω U are respectively the linear velocity and angular velocity of U T B described in the ultrasonic probe coordinate system U; τ c is the torque required to compensate for the dynamics of the ultrasonic robot; K c represents the stiffness gain matrix, and K d represents the damping gain matrix.

[0047] In some embodiments, controlling the ultrasonic robot according to the joint reference torque and the dynamic equation includes the following steps:

[0048] Substitute the joint reference torque as the joint input torque into the dynamic equation, subtract the joint impedance torque from the joint reference torque, and then control the ultrasonic robot according to the dynamic equation.

[0049] To achieve the above object, another aspect of the embodiments of the present application proposes an impedance control device for an ultrasonic robot, the device includes:

[0050] A dynamics modeling unit for performing dynamics modeling on the multi-degree-of-freedom robotic arm of an ultrasonic robot to obtain the Lagrangian form dynamics equation of the robotic arm in joint space; wherein, the dynamics equation includes impedance constraints;

[0051] A probe initialization unit for identifying human key points and then controlling the ultrasonic probe of the ultrasonic robot to move to the human key points;

[0052] A trajectory planning unit for performing trajectory planning based on the human key points to obtain the poses of the ultrasonic probe at multiple path points;

[0053] A torque determination unit for determining the joint reference torque according to the desired end force and the poses at each of the path points;

[0054] A robot drive unit for controlling the ultrasonic robot according to the joint reference torque and the dynamics equation.

[0055] To achieve the above object, another aspect of the embodiments of the present application provides an electronic device, which includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, the above method is implemented.

[0056] To achieve the above object, another aspect of the embodiments of the present application provides a computer-readable storage medium, which stores a computer program, and when the computer program is executed by a processor, the above method is implemented.

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

[0058] The present application can perform dynamics modeling on the multi-degree-of-freedom robotic arm of an ultrasonic robot to obtain the Lagrangian form dynamics equation of the robotic arm in joint space, wherein the dynamics equation includes impedance constraints; identify human key points and then control the ultrasonic probe of the ultrasonic robot to move to the human key points; perform trajectory planning based on the human key points to obtain the poses of the ultrasonic probe at multiple path points; determine the joint reference torque according to the desired end force and the poses at each path point; and control the ultrasonic robot according to the joint reference torque and the dynamics equation. The present application combines the dynamics modeling of the multi-degree-of-freedom robotic arm with joint impedance for control, and can achieve efficient and accurate scanning of complex body surfaces. Description of the Drawings

[0059] To more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the accompanying drawings required for the description of the embodiments. Obviously, the accompanying drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can be obtained based on these drawings.

[0060] Figure 1 A flowchart of an ultrasonic robot impedance control method provided by an embodiment of the present application;

[0061] Figure 2 A schematic diagram of the MDH coordinate system of an ultrasonic robotic arm provided by an embodiment of the present application;

[0062] Figure 3 A kinematic model diagram of an ultrasonic robot provided by an embodiment of the present application;

[0063] Figure 4 A joint impedance control block diagram provided by an embodiment of the present application;

[0064] Figure 5 An ultrasonic robot impedance control diagram provided by an embodiment of the present application;

[0065] Figure 6 A schematic diagram of the structure of an ultrasonic robot impedance control device provided by an embodiment of the present application;

[0066] Figure 7 A schematic diagram of the hardware structure of an electronic device provided by an embodiment of the present application. Detailed implementation manners

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

[0068] It can 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 this 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", "when" as used herein may be interpreted as "when...", "while...", or "in response to determining".

[0069] The terms "at least one", "a plurality of", "each", "any one", etc. used in this application, at least one includes one, two, or more than two, a plurality includes two or more than two, each refers to each of the corresponding plurality, and any one refers to any one of the plurality.

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

[0071] Referring to Figure 1 , the embodiments of this application provide an ultrasonic robot impedance control method, which may include but is not limited to including S100 to S140, specifically as follows:

[0072] S100: Perform dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in the joint space; wherein, the dynamic equation includes impedance constraints;

[0073] S110: Identify human key points, and then control the ultrasonic probe of the ultrasonic robot to move to the human key points;

[0074] S120: Perform trajectory planning according to the human key points to obtain the poses of the ultrasonic probe at multiple path points;

[0075] S130: Determine the joint reference torque according to the desired end force and the poses at each of the path points;

[0076] S140: Control the ultrasonic robot according to the joint reference torque and the dynamic equation.

[0077] Optionally, the performing dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in the joint space includes the following steps:

[0078] Dynamically model the multi - degree - of - freedom robotic arm of the ultrasonic robot to obtain the following Lagrangian - form dynamic equation of the robotic arm in the joint space:

[0079]

[0080] where M, C, and G are the mass matrix, centripetal force matrix, and gravity matrix respectively; θ, are the joint angle, joint angular velocity, and joint angular acceleration respectively; τ is the joint input torque, and τ e is the joint impedance torque.

[0081] Optionally, the steps to determine the joint impedance torque include the following steps:

[0082] Establish the joint impedance torque in the joint space as follows:

[0083]

[0084] where Δq is the error between the desired joint angle and the actual joint angle, is the error between the desired joint angular velocity and the actual joint angular velocity, is the error between the desired joint angular acceleration and the actual joint angular acceleration; M(q)=M d .

[0085] Optionally, the trajectory planning according to the human key points to obtain the poses of the ultrasonic probe at multiple path points includes the following steps:

[0086] Determine the initial point and the end point corresponding to the human key points;

[0087] Generate multiple equally - spaced points between the initial point and the end point;

[0088] Determine the normal vectors corresponding to each of the equally - spaced points;

[0089] Calculate the path points according to the normal vectors corresponding to each of the equally - spaced points;

[0090] Take the normal vector as the z i axis, calculate the x i axis according to the straight line connecting the current path point and the next path point, and then calculate the y i axis;

[0091] According to the z i axis, the x i axis and the y i axis, calculate the pose of the ultrasonic probe at the corresponding path point.

[0092] Optionally, determining the normal vectors corresponding to each of the equidistant points includes the following steps:

[0093] Determine the local surface of the point cloud within the preset range of each equidistant point; solve for the minimum eigenvalue of the covariance matrix of the local surface of the point cloud; determine the eigenvector corresponding to the minimum eigenvalue as the normal vector;

[0094] The covariance matrix is:

[0095]

[0096] where K i represents the local surface of the point cloud, m represents the number of selected points, represents the centroid, and X represents the covariance matrix;

[0097] Calculating the path points according to the normal vectors corresponding to each of the equidistant points includes the following steps:

[0098] Calculate the path points according to the normal vectors corresponding to each of the equidistant points as follows:

[0099]

[0100] where ρ i represents the path point, σ i represents the equidistant point, n represents the normal vector, and p i represents the point on the surface of the point cloud; P represents the set of points on the surface of the point cloud;

[0101] Calculating the x i axis based on the straight line connecting the current path point and the next path point, and then calculating the y i axis includes the following steps:

[0102] Calculate the straight line as:

[0103]

[0104] where yy i represents the straight line between the current path point and the next path point; ρ i represents the current path point; ρ i+1 represents the next path;

[0105] Calculate the x i axis as:

[0106]

[0107] Calculate the y i axis as:

[0108]

[0109] Optionally, determining the joint reference torque according to the desired end force and the poses at each of the path points includes the following steps:

[0110] Determine the joint reference torque according to the desired end force and the poses at each of the path points as follows:

[0111]

[0112] where τ r represents the joint reference torque; represents the Jacobian matrix of the robotic arm in the ultrasonic probe coordinate system U; represents the deviation between the reference pose and the current pose of the ultrasonic robot in the ultrasonic probe coordinate system U; and ω U are respectively described in the ultrasonic probe coordinate system U U T B linear velocity and angular velocity; τ c is the torque required to compensate for the dynamics of the ultrasonic robot; K c represents the stiffness gain matrix, and K d represents the damping gain matrix.

[0113] Optionally, controlling the ultrasonic robot according to the joint reference torque and the dynamic equation includes the following steps:

[0114] Substitute the joint reference torque as the joint input torque into the dynamic equation, subtract the joint impedance torque from the joint reference torque, and then control the ultrasonic robot according to the dynamic equation.

[0115] Next, specific application examples will be combined to introduce and illustrate the solutions of the embodiments of the present application in detail.

[0116] This embodiment proposes a six-degree-of-freedom ultrasonic robot impedance control method, which uses a force sensor to measure the external force at the end in real time and realizes the compliant control of the robotic arm through an impedance control strategy. This method can realize the efficient scanning of complex body surfaces by calculating the relationship between the external force and the end displacement in real time, and combining the dynamic modeling of the robotic arm and joint impedance control. By optimizing the impedance control strategy, this embodiment can effectively solve the stability problem faced by existing ultrasonic robots during autonomous scanning, improve the scanning efficiency and image quality, and enhance the safety during ultrasonic scanning.

[0117] This embodiment discloses an impedance control method for an ultrasonic robot. The autonomy of the ultrasonic robot enables it to assist physicians in completing ultrasonic scanning operations and perform auxiliary diagnoses in combination with ultrasonic images. However, there are still problems with the instability and safety of the contact force during the scanning of complex body surfaces by the robot. The technical method of this embodiment fully considers the above problems. The system of this embodiment mainly consists of three parts: kinematic modeling, dynamic modeling, and joint impedance control methods. By using the technology of this embodiment, the force control accuracy and stability of the ultrasonic robot during scanning can be improved, and the safety of the ultrasonic robot during autonomous ultrasonic scanning can be enhanced.

[0118] Specifically, this embodiment includes the following technical solutions:

[0119] 1. Kinematic modeling of the robot.

[0120] 1.1 Kinematic model of the robotic arm.

[0121] The kinematic model of the ultrasonic robotic arm is a mathematical model that describes the relationship between the pose of the end effector of the robotic arm and each joint of the robotic arm, and is divided into two parts: the forward kinematic model and the inverse kinematic model.

[0122] The forward kinematic model of the ultrasonic robotic arm describes how to solve the pose of the end effector in the base coordinate system given the joint angles of each joint of the robotic arm. The ultrasonic robotic arm used in this embodiment has 6 degrees of freedom. The forward kinematic model is derived by the MDH method, and 6 MDH coordinate systems are established at the joints, as Figure 2 shown, where {0}:{6} represents the MDH coordinate system of the 6-degree-of-freedom robotic arm, {d i}(i = 1, 4, 6) is the joint offset, and {a3} is the link length.

[0123] Let the joint angles of the 6-degree-of-freedom robotic arm be θ i , the link twist angle be α i , i-1 T i be the homogeneous transformation matrix from the {i - 1} coordinate system to the {i} coordinate system, i = 1, 2, L, 6, then i-1 T i is expressed as:

[0124]

[0125] According to the chain rule, the forward kinematic model of the ultrasonic robotic arm is as follows:

[0126] 0 T6(θ1, θ2,..., θ6) = 0 T1 1 T2L 5 T6 (2)

[0127] The inverse kinematic model of the ultrasonic robotic arm describes how to solve the joint angles of the robotic arm given the target pose of the end effector in the base coordinate system. Since the ultrasonic robotic arm of this embodiment satisfies the Pieper criterion, that is, the three adjacent joint axes of the robotic arm intersect at a point, a closed-form solution can be calculated according to the inverse kinematics. The last three joint axes of the ultrasonic robotic arm of this embodiment intersect at a point, that is, the three axes of coordinate systems {4}, {5}, and {6} intersect at a point. Then the homogeneous coordinates of this point in the base coordinate system of the robotic arm can be expressed as:

[0128]

[0129] As can be seen from Equation (1) 3 p4 = [a3 - d4sinα3 d4cosα3 1] T , substituting Equation (1) into Equation (3), we can get 0 The expression of p4 with respect to (θ1, θ2, θ3), given 0 the coordinate values of p6, then the coordinate values of p4 can be obtained accordingly 0 , and then list 0 the sum-of-squares equation of p4, and first solve for θ3, then solve for θ2, and finally solve for θ1 by the variable substitution method. And the axes of the last three joints of the robotic arm intersect at a point, and the rotations of these three joints will affect the end pose of the robotic arm. The (θ4, θ5, θ6) can be solved from the rotation matrix 0 R6 representing the end pose.

[0130] 1.2. System kinematic model.

[0131] The ultrasonic robot system consists of a robotic arm, an ultrasonic probe, a probe connector, a six-axis force sensor, an IMU, and a global camera. Among them, the robotic arm is a 6-degree-of-freedom robotic arm; the ultrasonic probe is a linear probe used for imaging of shallow tissues and blood vessels, and the imaging mode is the B mode, which will convert the echo intensity of the ultrasonic waves emitted by the ultrasonic probe into a grayscale image; the probe connector is manufactured by 3D printing; the six-axis force sensor is used to quickly and accurately measure the forces and torques on the three coordinate axes x, y, and z of the sensor; the IMU is an inertial measurement unit, usually composed of a three-axis accelerometer and a three-axis gyroscope, used to measure the linear acceleration and angular velocity of the ultrasonic probe in space, so as to obtain its pose and motion trajectory. The global camera is an RGB-D camera used to reconstruct the surface of the human tissue to be scanned and provide visual input information for the trajectory planning of the robot.

[0132] Perform kinematic modeling on the entire ultrasonic robot system, as Figure 3As shown in the figure, {base} is the base coordinate system of the robot, {end} is the end coordinate system of the robot, {imu} is the IMU coordinate system, {img} is the image coordinate system of the ultrasound probe, and {gc} is the coordinate system of the global camera. According to the forward kinematics model of the robot arm, it can be solved in real time to obtain base T end , can be obtained in real time through IMU base T imu ; Through the multi-target calibration method, we can get end T imu , imu T img . Finally, the kinematic model of the entire system is constructed.

[0133] 2. Robot dynamics modeling.

[0134] 2.1. Gravity compensation.

[0135] The six-dimensional force sensor is installed at the end of the six-degree-of-freedom robot arm to measure the force and torque of the three coordinate axes xyz. Since the gravity of the connector and the ultrasonic probe will affect the measurement of the end operating force, gravity compensation is required before using the force sensor. The measurement value of the known force sensor is F = (F x ,F y ,F z ,M x ,M y ,M z ); When the force sensor is unloaded, the zero point value of the force sensor is F 0 =(F 0 x ,F 0 y ,F 0 z ,M 0 x ,M 0 y ,M 0 z ); the measured values of the force and torque of the connector and the ultrasonic probe in the force sensor coordinate system are F s =(F s x ,F s y ,F s z ,M s x ,M s y ,M s z ), where the moment component is obtained from the rigid body mechanics equation M = r × F:

[0136] M s x = r y F z -r z F y

[0137] M s y = r y F x -r x F z

[0138] M s z = r x F y -r y F x (4)

[0139] where r = (r x , r y , r z ) is the spatial coordinate of the centroid of the end effector in the coordinate system of the six - axis force sensor.

[0140] The formula for the true contact force between the end effector and the environment is:

[0141] F t = F - F 0 - F (5)

[0142] 2.2. Kinetic modeling.

[0143] Based on the Newton - Euler equations, the iterative kinetic equations of a six - degree - of - freedom manipulator are established. First, the forward recursion of the velocity and acceleration of the manipulator is carried out. According to the motion of each joint, the linear velocity and angular velocity i v i , i ω i of the link {i} are obtained. Then, in the {i} - th coordinate system, the linear velocity i v i+1 and angular velocity i ω i+1 of the (i + 1) - th link are given by the following formulas:

[0144]

[0145] i v i+1 = i ω i × i P i+1 + i v i (7)

[0146] Among them, i R i+1 represents the rotation matrix of the coordinate system {i + 1} relative to the {i} coordinate system, represents the angular velocity of the connecting rod in the z-axis direction; i P i+1 represents the position vector from the origin of the coordinate system {i} to the origin of the coordinate system {i + 1}.

[0147] For equations (6) and (7), in the {i} coordinate system, the linear acceleration and angular acceleration

[0148]

[0149]

[0150] Among them, respectively represent the angular acceleration and linear acceleration of the (i + 1)-th link in the {i} coordinate system.

[0151] Performing coordinate transformation on equations (8) and (9), the angular acceleration and linear acceleration transmission relationship of the (i + 1)-th link in the (i + 1) coordinate system can be obtained:

[0152]

[0153] Among them, i+1 R i represents the rotation matrix of the coordinate system {i + 1} relative to the {i} coordinate system, represents the angular acceleration of the connecting rod {i + 1} in the z-axis direction, respectively represent the angular acceleration and linear acceleration of the {i}-th link in the {i} coordinate system.

[0154] The linear acceleration at the center of mass of the connecting rod is:

[0155]

[0156] Among them, represents the position vector from the origin of the coordinate system {i + 1} to the center of mass of the connecting rod {i + 1}.

[0157] From Newton's equation F = mv c and Euler's equation the inertial force i+1 F i+1 and moment i+1 N i+1 at the center of mass of the (i + 1)-th link of the robotic arm can be obtained:

[0158]

[0159] wherein, m i+1 represents the mass of the connecting rod, and is the moment of inertia of the connecting rod.

[0160] For the connecting rod {i}, the force balance equation can be obtained:

[0161] i f i = i R i+1 i+1 f i+1 + i F i (15)

[0162]

[0163] wherein, i f i represents the resultant force acting on the connecting rod {i}, i F i represents the inertia force of the connecting rod {i}, i n i represents the total torque acting on the connecting rod {i}, i N i represents the inertia torque of the connecting rod {i}, i+1 n i+1 represents the torque of the connecting rod {i+1}, represents the torque generated by the inertia force, i P i+1 ×( i R i+1 i+1 f i+1 ) represents the torque generated by the force of the connecting rod {i+1}, τ i represents the joint torque.

[0164] Substituting into Equation (17) and expanding, the derived recursive form of the dynamic equation can be sorted out to obtain the Lagrangian form of the dynamic equation in the joint space:

[0165]

[0166] wherein, M, C, and G are the mass, centripetal / Coriolis force, and gravity matrices respectively; θ, are the joint angle, joint angular velocity, and joint angular acceleration respectively; τ is the joint input torque, and τ e is the joint impedance torque.

[0167] 3. Ultrasonic robot joint impedance control.

[0168] 3.1 Weak Coupling Joint Impedance Control.

[0169] Based on the dynamic model of the six-degree-of-freedom robotic arm, this section studies joint impedance control to achieve the compliance of the ultrasonic robot.

[0170] Joint impedance control is a control method that controls the movement of the robotic arm and its interaction with the environment by regulating the relationship between joint force and displacement. Different from traditional trajectory tracking, joint impedance control focuses on the dynamic response of the joints. It controls the relationship between the force and displacement of each joint by establishing a virtual spring-damper-mass system. This control method takes into account the inertia, damping, and stiffness of the robotic arm and can adjust these parameters at the joint level to achieve different motion characteristics.

[0171] To make the joints of the ultrasonic robot compliant, the joint impedance torque can be established in the joint space:

[0172]

[0173] where, Δq is the error between the desired joint angle and the actual joint angle, is the error between the desired joint angular velocity and the actual joint angular velocity, is the error between the desired joint angular acceleration and the actual joint angular acceleration.

[0174] According to the dynamic model of the robot, the desired joint input torque is:

[0175]

[0176] Since the joint acceleration and joint torque cannot be directly measured, this control law cannot be directly used. Design M(q) = M d , simplify the desired joint torque input, and obtain the joint impedance control block diagram as shown in Figure 4 . At this time, the joint input torque is:

[0177]

[0178] Adopting the impedance control law of Equation (21), the joint impedance torque becomes:

[0179]

[0180] Compared with the ideal impedance model, the above formula cannot track the desired inertial characteristics, and there is coupling in the terms related to mass. However, since the joint angular acceleration of the robot is generally small and has a very limited impact on the system, under the control of Equation (22), the robot joints can still possess the impedance characteristics of stiffness and damping, and can avoid the problem of joint angular acceleration measurement, reducing the system complexity and having practicality.

[0181] 4. Ultrasonic robot impedance control.

[0182] 4.1. Human key point recognition.

[0183] In this embodiment, taking thyroid ultrasound examination as an example, the thyroid tissue is located below the neck of the human body and consists of two left and right lobes and an isthmus. The neck of the human body is one of the key features of the human body. Therefore, generally, the subject is made to lie supine or semi-supine, and the optical camera can be placed near the robotic arm or fixed beside the bed to clearly capture the neck area. Based on the deep learning neural network, the human key points are recognized through the optical camera, and key points such as the head, mandible, collarbone, and neck are recognized, and the approximate range of the target area is inferred through geometric relationships. This embodiment uses the key feature points to roughly locate the anatomical structure of the thyroid, so as to guide the ultrasonic robot to move the ultrasonic probe to the specified position, facilitating the subsequent ultrasonic scanning operation path planning and control.

[0184] 4.2. Scanning trajectory planning.

[0185] From Section 4.1, the initial target area for the ultrasonic probe to scan can already be obtained. Next, the path trajectory for scanning the target area is required. The proposed trajectory planning method can autonomously plan the path that the robotic arm needs to follow during scanning. Taking thyroid scanning as an example, first, the optical camera obtains the point cloud P of the human neck. According to the neck key points k∈R 3 recognized by the optical camera and the nearby point cloud P, the initial point l i ∈R 3 of the thyroid scanning area and the termination point l e ∈R 3 are given in combination with empirical parameters.

[0186] The specific trajectory planning method steps are as follows:

[0187] A series of equally spaced points σ i are generated between the initial point l e and the termination point l i , where (i = 1,..., s), and d is the maximum allowable distance between any two points. The number s of the generated points and the spacing d between them are determined by the size of the ultrasonic probe.

[0188] Since the human body surface is a curved surface and there are certain errors in the optical camera, the generated equally spaced points must be projected onto the actual human body surface. By projecting each equally spaced point σ i onto the nearest point cloud surface point p i along the normal vector direction, the normal vector n is the normal vector of the local surface k i of the point cloud near the equally spaced point σ i , and the normal vector of the local surface k iCovariance matrix:

[0189]

[0190] where m represents the number of selected points, and K i local surface of the point cloud represents the centroid.

[0191] Calculate the eigenvalues and eigenvectors of the covariance matrix X. The eigenvector corresponding to the smallest eigenvalue is the required normal vector n.

[0192] Calculate the path point ρ i :

[0193]

[0194] Calculate the pose of the ultrasonic probe. Use the equidistant point normal vector as the z i axis, connect the current path point ρ i and the next path point ρ i+1 as the y i axis, calculate the x i axis, and finally calculate the y i axis:

[0195]

[0196] Through the above steps, the poses of a series of path points of the ultrasonic probe relative to the base coordinate system B can be obtained

[0197] 4.3 Cartesian space impedance control.

[0198] The purpose of impedance control is to establish the dynamic relationship between the target motion trajectory of the robot and the contact force, rather than just achieving trajectory tracking. Its dynamic characteristics are usually described by a mass-damping-spring second-order system model, which is determined by three parameters: the inertia coefficient, the damping coefficient, and the stiffness coefficient. By adjusting these parameters, different impedance characteristics can be achieved, such as high rigidity, high damping, or high inertia. Intuitively, impedance control can be understood as setting a virtual impedance system between the actual position and the desired position of the robotic arm, and controlling the robotic arm to track the force output of this virtual system. Since the impedance system has "compliance", it makes the movement of the robotic arm smoother and reduces the impact on the environment.

[0199] In order to make the ultrasonic robot have "compliance" and ensure that the contact force between the probe and the human body surface can present a clearer image, this embodiment proposes an impedance control algorithm to ensure that the distal surface of the probe must always be in full contact with the human body surface to ensure a clear image, and the contact force is controlled within a safe range.

[0200] The inputs of the proposed impedance control method include: the desired end-effector force F ed , the desired pose of the robotic arm for path planning the obtained Cartesian position vector and the orientation quaternion The output of the proposed impedance control method is the joint reference torque τ r , which is passed to the underlying controller built into the robotic arm to control the joint torques.

[0201] When the Jacobian matrix of the robotic arm inverse kinematics is non-invertible, the damped Moore-Penrose pseudoinverse is used:

[0202]

[0203] where J B is the Jacobian matrix of the robotic arm, is the damping coefficient, I is the identity matrix, is the Jacobian pseudoinverse matrix of the robotic arm.

[0204] A continuous motion trajectory containing several ultrasonic reference intermediate poses is generated between discrete pose sets, and these reference poses are defined by the position vector ref p B and the orientation quaternion ref q B to smoothly provide inputs to the controller:

[0205] ( ref p B , ref q B ) = F FK (θ m )(29)

[0206]

[0207] where, e m represents the deviation between the robotic arm path point pose and the reference pose; F FK (·) is the forward kinematics function; K p represents the control gain matrix.

[0208] The impedance control is performed in the coordinate system U of the ultrasonic probe, and the following control law is adopted:

[0209]

[0210] where, τ r represents the joint reference torque; represents the Jacobian matrix of the robotic arm in the coordinate system U of the ultrasonic probe; Represents the deviation between the robot reference pose and the current pose in the ultrasonic probe coordinate system U; and ω U are respectively the linear velocity and angular velocity of U T B described in the ultrasonic probe coordinate system U; τ c is the torque required to compensate for the robot dynamics. This torque is calculated according to the dynamic model of the robot itself (i.e., compensating for the inertia, Coriolis force, and gravity of the robot), and acts together with the above impedance control terms to achieve safe and stable probe contact and positioning.

[0211] Using the gain matrices K c and K d , to define the stiffness and damping of the force-position relationship. The stiffness K c and damping K d are defined as follows:

[0212] K c =diag([K x ,K y ,K z ,K i ,K j ,K k )(32)

[0213]

[0214] Through the force-position relationship provided by impedance control, the robot has corresponding compliance characteristics in the presence of contact forces. At the instantaneous position reference p U of axis, a constant offset δz∈R + is introduced to achieve continuous contact between the probe and the surface and generate a vertical acting force The component of this force at steady state can be modeled as:

[0215]

[0216] where, represents the error between the surface position constructed by the point cloud and the actual surface position.

[0217] When , the probe will continuously maintain contact with the target surface. By adjusting the coefficient K z , the range of the contact force can be selected to ensure that the end probe makes sufficient contact with the human body surface to present a clear ultrasonic image while ensuring safety. Exemplarily, the impedance control diagram of the ultrasonic robot is as shown in Figure 5 Figure.

[0218] Refer to Figure 6, The embodiments of the present application also provide an impedance control device for an ultrasonic robot, which can implement the above-mentioned impedance control method for an ultrasonic robot. The device includes:

[0219] A dynamics modeling unit for performing dynamics modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamics equation of the robotic arm in the joint space; wherein, the dynamics equation includes impedance constraints;

[0220] A probe initialization unit for identifying key human body points, and then controlling the ultrasonic probe of the ultrasonic robot to move to the key human body points;

[0221] A trajectory planning unit for performing trajectory planning according to the key human body points to obtain the poses of the ultrasonic probe at multiple path points;

[0222] A torque determination unit for determining the joint reference torque according to the desired end force and the poses at each of the path points;

[0223] A robot driving unit for controlling the ultrasonic robot according to the joint reference torque and the dynamics equation.

[0224] It can be understood that the content in the above method embodiments is applicable to the device embodiments of the present application. The functions specifically implemented by the device embodiments of the present application are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those of the above method embodiments.

[0225] The embodiments of the present application also provide an electronic device. The electronic device includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, the method of the embodiments of the present application is implemented. The electronic device can be any intelligent terminal including a tablet computer, an in-vehicle computer, etc.

[0226] It can be understood that the content in the above method embodiments is applicable to the device embodiments of the present application. The functions specifically implemented by the device embodiments of the present application are the same as those of the method of the present application, and the beneficial effects achieved are also the same as those of the method of the present application.

[0227] Please refer to Figure 7 , Figure 7 which schematically shows the hardware structure of an electronic device in another embodiment. The electronic device includes:

[0228] The processor 701 can be implemented in the form of a general-purpose CPU (Central Processing Unit), a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, etc., and is used to execute relevant programs to implement the technical solutions provided in the embodiments of the present application.

[0229] The memory 702 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM), etc. The memory 702 can store an operating system and other application programs. When implementing the technical solutions provided in the embodiments of this specification through software or firmware, the relevant program codes are stored in the memory 702 and are called by the processor 701 to execute the methods in the embodiments of the present application.

[0230] The input / output interface 703 is used to implement information input and output.

[0231] The communication interface 704 is used to implement communication interaction between this device and other devices, and can implement communication through a wired method (such as USB, network cable, etc.) or through a wireless method (such as mobile network, WIFI, Bluetooth, etc.).

[0232] The bus 705 transmits information between various components of the device (such as the processor 701, the memory 702, the input / output interface 703, and the communication interface 704).

[0233] Among them, the processor 701, the memory 702, the input / output interface 703, and the communication interface 704 are communicatively connected to each other inside the device through the bus 705.

[0234] The embodiments of the present application also provide a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the method of the present application.

[0235] It can be understood that the content in the above method embodiments is applicable to the storage medium embodiments of the present application. The functions specifically implemented by the storage medium embodiments of the present application are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those of the above method embodiments.

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

[0237] The embodiments described in the embodiments of the present application are for more clearly illustrating the technical solutions of the embodiments of the present application, and do not constitute a limitation on the technical solutions provided by the embodiments of the present application. Those skilled in the art will know that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided by the embodiments of the present application are equally applicable to similar technical problems.

[0238] Those skilled in the art can 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 those shown, or combine certain steps, or different steps.

[0239] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, that is, they may be located in one place, or may be distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0240] Those of ordinary skill in the art can understand that all or some of the steps in the methods disclosed above, and the functional modules / units in the systems and devices can be implemented as software, firmware, hardware, and appropriate combinations thereof.

[0241] The terms "first", "second", "third", "fourth", etc. (if any) in the specification of the present application and the above drawings are used to distinguish similar objects, and do not have to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present application described here can be implemented in an order other than those illustrated or described here. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units does not have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products, or devices.

[0242] It should be understood that in this application, "at least one (item)" means one or more, and "a plurality" means two or more. "And / or" is used to describe the relationship between associated objects and indicates that there can be three relationships. For example, "A and / or B" can mean: only A exists, only B exists, and both A and B exist simultaneously. Here, A and B can be singular or plural. The character " / " generally indicates an "or" relationship between the associated objects before and after. "At least one (item) of the following" or similar expressions refer to any combination of these items, including any combination of single items or plural items. For example, at least one (item) 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, and c can be single or multiple.

[0243] In several embodiments provided in this application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the above division of units is only a logical function division. In actual implementation, there can be other division methods. For example, 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 displayed or discussed coupling or direct coupling or communication connection between each other can be through some interfaces. The indirect coupling or communication connection of devices or units can be in electrical, mechanical, or other forms.

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

[0245] In addition, in each embodiment of this application, each functional unit can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.

[0246] When 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 this application, in essence, or the part that makes a contribution to the prior art, or all or part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes multiple instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods of various embodiments of this application. The foregoing storage medium includes: various media that can store programs such as USB flash drives, mobile hard disks, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical discs.

[0247] The preferred embodiments of the embodiments of this application have been described above with reference to the accompanying drawings. However, this does not limit the scope of the rights of the embodiments of this application. Any modifications, equivalent replacements, and improvements made by those skilled in the art without departing from the scope and essence of the embodiments of this application shall fall within the scope of the rights of the embodiments of this application.

Claims

1. An impedance control method for an ultrasonic robot, characterized in that, The method includes the following steps: Perform dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in joint space; wherein, the dynamic equation includes impedance constraints; Identify human key points, and then control the ultrasonic probe of the ultrasonic robot to move to the human key points; Perform trajectory planning based on the human key points to obtain the poses of the ultrasonic probe at multiple path points; Determine the joint reference torque according to the desired end force and the poses at each of the path points; Control the ultrasonic robot according to the joint reference torque and the dynamic equation.

2. The ultrasonic robot impedance control method according to claim 1, characterized in that The performing dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in joint space includes the following steps: Perform dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in joint space as follows: where M, C, and G are the mass matrix, centripetal force matrix, and gravity matrix, respectively; θ, are the joint angle, joint angular velocity, and joint angular acceleration, respectively; τ is the joint input torque, and τ e is the joint impedance torque.

3. The ultrasonic robot impedance control method according to claim 2, wherein, The steps of determining the joint impedance torque include the following steps: Establish the joint impedance torque in joint space as follows: where Δq is the error between the desired joint angle and the actual joint angle, is the error between the desired joint angular velocity and the actual joint angular velocity, is the error between the desired joint angular acceleration and the actual joint angular acceleration; M(q) = M d .

4. The ultrasonic robot impedance control method according to claim 1, wherein The performing trajectory planning based on the human key points to obtain the poses of the ultrasonic probe at multiple path points includes the following steps: Determine the initial point and the end point corresponding to the human key points; Generate multiple equally spaced points between the initial point and the end point; Determine the normal vectors corresponding to each of the equally spaced points; Calculate the path points according to the normal vectors corresponding to each of the equally spaced points; Use the normal vector as the z i axis, calculate the x i axis according to the straight line connecting the current path point and the next path point, and then calculate the y i axis; According to the z i axis, the x i axis and the y i axis, calculate the pose of the ultrasonic probe at the corresponding path point.

5. The ultrasonic robot impedance control method according to claim 4, characterized in that, The determining the normal vectors corresponding to each of the equally spaced points includes the following steps: Determine the local surface of the point cloud within the preset range of each of the equally spaced points; solve the minimum eigenvalue of the covariance matrix of the local surface of the point cloud; determine the eigenvector corresponding to the minimum eigenvalue as the normal vector; The covariance matrix is: Among them, K i represents the local surface of the point cloud, m represents the number of selected points, represents the centroid, and X represents the covariance matrix; The calculating the path points according to the normal vectors corresponding to each of the equally spaced points includes the following steps: Calculate the path points according to the normal vectors corresponding to each of the equally spaced points as follows: Among them, ρ i represents the path point, σ i represents the equidistant point, n represents the normal vector, p i represents a point cloud surface point; P represents a set of point cloud surface points; Calculate x based on the straight line connecting the current path point and the next path point, and then calculate y axis, including the following steps: i axis, and then calculate the y i axis, including the following steps: Calculate the straight line as: Among them, yy i represents the straight line of the current path point and the next path point; ρ i represents the current path point; ρ i+1 represents the next path point; Calculate the x i axis is: Calculate the said y i axis is:

6. The ultrasonic robot impedance control method according to claim 1, wherein, The determining the joint reference torque according to the desired end force and the poses at each of the path points includes the following steps: Determine the joint reference torque according to the desired end force and the poses at each of the path points as follows: Among them, τ r represents the joint reference torque; represents the Jacobian matrix of the robotic arm in the ultrasonic probe coordinate system U; represents the deviation between the reference pose and the current pose of the ultrasonic robot in the ultrasonic probe coordinate system U; and ω U are respectively described in the ultrasonic probe coordinate system U U T B linear velocity and angular velocity; τ c is the torque required to compensate for the dynamics of the ultrasonic robot; K c represents the stiffness gain matrix, and K d represents the damping gain matrix.

7. The ultrasonic robot impedance control method according to claim 2, characterized in that The controlling the ultrasonic robot according to the joint reference torque and the dynamic equation includes the following steps: Substitute the joint reference torque as the joint input torque into the dynamic equation, subtract the joint impedance torque from the joint reference torque, and then control the ultrasonic robot according to the dynamic equation.

8. An ultrasonic robot impedance control device, characterized in that The device includes: A dynamic modeling unit for performing dynamic modeling on the multi-degree-of-freedom robotic arm of the ultrasonic robot to obtain the Lagrangian form dynamic equation of the robotic arm in joint space; wherein, the dynamic equation includes impedance constraints; A probe initialization unit for identifying human key points and then controlling the ultrasonic probe of the ultrasonic robot to move to the human key points; A trajectory planning unit for performing trajectory planning based on the human key points to obtain the poses of the ultrasonic probe at multiple path points; A torque determination unit for determining a joint reference torque according to an expected end force and poses at each of the path points; A robot drive unit for controlling the ultrasonic robot according to the joint reference torque and the dynamic equation.

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

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

Images