Zero-force dragging method and device for autonomous ultrasonic robot

Abstract

The invention discloses a zero-force dragging method and device for an autonomous ultrasonic robot. The method comprises the steps that second mechanical data corresponding to three directions in a world coordinate system are obtained; obtaining third mechanical data; based on a first preset value, a preset parameter, each piece of second mechanical data and the corresponding third mechanical data, displacement variations in the three directions are obtained through calculation, and the positions of the three directions at the next moment are obtained in combination with the obtained current positions in the three directions; determining a corresponding minimum boundary and a maximum boundary; obtaining a minimum boundary and a maximum boundary in a preset Y direction; for the position of the next moment in each direction, if the position is smaller than the corresponding minimum boundary, updating the position to the corresponding minimum boundary, and if the position is larger than the corresponding maximum boundary, updating the position to the corresponding maximum boundary to obtain a predicted position; and controlling the ultrasonic probe to move to the predicted positions in the three directions. The mechanical arm can be accurately controlled to achieve zero-force dragging, and safety is guaranteed.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of medical robot ultrasound scanning, in particular to a zero-force dragging method and device of an autonomous ultrasound robot. BACKGROUND

[0002] The autonomous ultrasound robot relies on a mechanical arm to carry an ultrasound probe, and deeply integrates artificial intelligence and a perception system, thereby realizing automatic scanning of ultrasound examination. This innovative technology is committed to solving the problems of high dependence on physician experience and difficulty in ensuring operation consistency in traditional ultrasound examination.

[0003] Zero-force dragging technology is a core element of human-machine collaboration. Through zero-force dragging technology, the mechanical arm realizes the function of "zero-force dragging" (or "compliant traction" and "guidance teaching"), which enables a person to directly manually move the end effector of the mechanical arm with minimal resistance. It is a key technology in the field of human-robot collaboration (HCR), and its core goal is to provide a natural, intuitive, and low-fatigue physical human-machine interaction experience, which is a key prerequisite for safe and flexible control. SUMMARY

[0004] In order to accurately control the mechanical arm to realize zero-force dragging and ensure safety, the present application provides a zero-force dragging method and device of an autonomous ultrasound robot.

[0005] In a first aspect, the present application provides a zero-force dragging method of an autonomous ultrasound robot, comprising: Based on the obtained rotation matrix and the first mechanical data corresponding to three directions in the tool coordinate system, the second mechanical data corresponding to three directions in the world coordinate system is calculated; Each of the second mechanical data is processed by using a preset PID control algorithm to obtain corresponding third mechanical data; Based on the first preset value, the preset parameter, each of the second mechanical data and the corresponding third mechanical data, the displacement change amount of three directions is calculated, and combined with the obtained current position of three directions, the next time position of three directions is obtained; Based on the preset boundary threshold value corresponding to the X direction and the Z direction, the current position and the next time position, the minimum boundary and the maximum boundary corresponding to each direction are determined; The minimum boundary and the maximum boundary of the Y direction are obtained; For the next time position of each direction, if it is less than the corresponding minimum boundary, it is updated to the corresponding minimum boundary, and if it is greater than the corresponding maximum boundary, it is updated to the corresponding maximum boundary to obtain a predicted position; The ultrasound probe is controlled to move to the predicted position of three directions.

[0006] Optionally, the processing each of the second mechanical data by using a preset PID control algorithm to obtain corresponding third mechanical data comprises: For each direction, an error amount is obtained based on a difference between an absolute value of the corresponding second mechanical data and a second preset value, wherein the absolute value of the second mechanical data is greater than the second preset value; A proportional gain is calculated based on a preset proportional gain reference value and the error amount; A proportional term is obtained based on a product between the proportional gain and the error amount; A current integral term is calculated based on a preset integral gain, the error amount and an obtained integral term at a previous time; A current differential term is calculated based on the second mechanical data, an obtained second mechanical data at a previous time and a preset differential gain; An output value is calculated based on the proportional term, the current integral term and the current differential term; If the second mechanical data is greater than the second preset value, a negative number of the output value is taken as the third mechanical data; if the second mechanical data is less than the second preset value, the output value is taken as the third mechanical data.

[0007] Optionally, the proportional gain reference value comprises a minimum proportional gain, a maximum ratio and a unit gain coefficient; The calculating the proportional gain based on the preset proportional gain reference value and the error amount comprises: The proportional gain is calculated based on the minimum proportional gain, the maximum ratio, the unit gain coefficient and the error amount according to the following formula: wherein, represents the proportional gain; represents the minimum proportional gain; represents the maximum ratio; represents the unit gain coefficient; represents the error amount.

[0008] Optionally, the preset boundary threshold value of the X direction comprises a first minimum value, a first maximum value and a boundary reference value; The preset boundary threshold value of the Z direction comprises a second minimum value, a second maximum value and a height threshold value; The determining a corresponding minimum boundary and a maximum boundary based on the obtained preset boundary threshold value corresponding to the X direction and the Z direction respectively, the current position and the next time position comprises: Based on the first minimum value, the first maximum value, and the boundary reference value, the minimum boundary threshold and the maximum boundary threshold are calculated. For the X direction, if the corresponding current position is less than or equal to the minimum boundary threshold and the next position is greater than the maximum boundary threshold, then the first minimum value is taken as the minimum boundary and the maximum boundary threshold is taken as the maximum boundary; if the corresponding current position is greater than or equal to the minimum boundary threshold and the next position is less than the minimum boundary threshold, then the minimum boundary threshold is taken as the minimum boundary and the first maximum value is taken as the maximum boundary; otherwise, the first minimum value is taken as the minimum boundary and the first maximum value is taken as the maximum boundary. For the Z direction, if the corresponding current position is less than or equal to the height threshold, then the height threshold is used as the minimum boundary and the second maximum value is used as the maximum boundary; otherwise, the second minimum value is used as the minimum boundary and the second maximum value is used as the maximum boundary.

[0009] Optionally, the step of calculating the minimum boundary threshold and the maximum boundary threshold based on the first minimum value, the first maximum value, and the boundary reference value includes: Based on the first minimum value, the first maximum value, and the boundary reference value, the minimum boundary threshold and the maximum boundary threshold are calculated according to the following formula: in, Indicates the minimum boundary threshold; Indicates the maximum boundary threshold; Indicates the first minimum value; Indicates the first maximum value; Indicates the boundary reference value.

[0010] Optionally, the preset parameters include a first parameter and a second parameter; Based on the first preset value, preset parameters, each of the second mechanical data and the corresponding third mechanical data, the displacement changes in three directions are calculated, and combined with the current positions in the three directions, the next position in the three directions is obtained, including: For each direction, if the corresponding second mechanical data is less than or equal to the first preset value, then the third mechanical data is used as the displacement change; otherwise, based on the third mechanical data, the second mechanical data, the first parameter, and the second parameter, the corresponding displacement change is calculated according to the following formula: in, This represents the change in displacement along the i-th direction; The third mechanical data represents the i-th direction; The second mechanical data represents the i-direction; A represents the first parameter; B represents the second parameter; The positions in the three directions at the next moment are obtained by summing the current positions and displacement changes in the three directions.

[0011] Secondly, embodiments of the present invention provide a zero-force dragging device for an autonomous ultrasonic robot, comprising: The first data processing module is used to calculate the second mechanical data corresponding to the three directions in the world coordinate system based on the acquired rotation matrix and the first mechanical data corresponding to the three directions in the real-time acquired tool coordinate system. The second data processing module is used to process each of the second mechanical data using a preset PID control algorithm to obtain the corresponding third mechanical data. The position determination module is used to calculate the displacement changes in three directions based on a first preset value, preset parameters, each second mechanical data and the corresponding third mechanical data, and to obtain the next position in the three directions by combining the current position in the three directions. The first boundary determination module is used to determine the corresponding minimum boundary and maximum boundary based on the preset boundary thresholds corresponding to the X and Z directions respectively, the current position and the next time position; The second boundary determination module is used to obtain the preset minimum and maximum boundaries in the Y direction; The data update module is used to update the position of the next time step in each direction to the corresponding minimum boundary if it is less than the corresponding minimum boundary, and to update it to the corresponding maximum boundary if it is greater than the corresponding maximum boundary, thus obtaining the predicted position. The control module is used to control the movement of the ultrasound probe to the predicted position in three directions.

[0012] Thirdly, embodiments of the present invention provide a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the zero-force dragging method for an autonomous ultrasonic robot as described in the first aspect.

[0013] Fourthly, embodiments of the present invention provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the zero-force dragging method for an autonomous ultrasonic robot as described in the first aspect.

[0014] Fifthly, embodiments of the present invention provide a computer program product containing instructions that, when the computer program product is run on a computer device, cause the computer device to execute the zero-force dragging method for an autonomous ultrasonic robot as described in the first aspect.

[0015] The beneficial effects of the above-mentioned technical solutions provided in the embodiments of the present invention include at least the following: This invention provides a zero-force dragging method for an autonomous ultrasonic robot. During data processing, a rotation matrix is ​​used to transform the first mechanical data in three directions of the tool coordinate system into the second mechanical data in three directions of the world coordinate system. This real-time transformation of the first mechanical data to the world coordinate system solves the control deviation caused by changes in the robot arm's posture in existing joint space admittance control methods. A PID control algorithm is used to optimize the second mechanical data to obtain the third mechanical data. Combined with a first preset value, preset parameters, the second mechanical data, and the current position, the position at the next moment is calculated, precisely controlling the robot arm to achieve zero-force dragging and achieving coordinated optimization of motion smoothness and stability. Dynamic constraints are applied to the X and Z directions, and a preset safety boundary is used in the Y direction. The system first determines whether the position at the next moment is within the safety boundary; if not, adjustments are made promptly, and then the ultrasonic probe is moved according to the predicted position. Predicting the safety of the position at the next moment before controlling the ultrasonic probe movement effectively prevents the robot arm from triggering motion limits or colliding, ensuring safety.

[0016] Other features and advantages of the invention will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in the written description and the accompanying drawings.

[0017] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0018] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is an example diagram of the ultrasonic robot provided in an embodiment of the present invention; Figure 2 This is a flowchart of the zero-force dragging method for an autonomous ultrasonic robot provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the zero-force dragging device for an autonomous ultrasonic robot provided in an embodiment of the present invention. Detailed Implementation

[0019] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.

[0020] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," "outer," "far," "near," "front," and "rear," etc., indicating the orientation or positional relationship, are based on the orientation or positional relationship shown in the accompanying drawings and are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0021] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0022] The inventors discovered that existing zero-force dragging technologies typically employ joint spatial admittance control to achieve this effect. Admittance control senses external forces and adjusts joint torque commands in real time, enabling the robotic arm to overcome interference from gravity, friction, and other factors during dragging, thus achieving a zero-force dragging effect. Its core lies in real-time compensation for gravity and friction torques, ensuring the joints respond only to external forces and are unaffected by inertial forces during dragging. However, this method processes force data in the tool coordinate system without real-time conversion to the world coordinate system, causing spatial motion accuracy to be affected by the robotic arm's posture. Furthermore, this method uses posterior-based collision detection (such as based on torque abrupt changes or distance sensors), requiring a response based on real-time sensor data. This reliance on detection speed may lead to response delays and compromised safety.

[0023] To address the aforementioned issues, the inventors developed a method and device for zero-force dragging of an autonomous ultrasonic robot, which can precisely control the robotic arm to achieve zero-force dragging while ensuring safety.

[0024] Example 1 This embodiment proposes a zero-force dragging method for an autonomous ultrasonic robot, which can be applied to, for example... Figure 1 The ultrasonic robot shown may include a robotic arm and a computer. The robotic arm is equipped with an ultrasonic probe for acquiring ultrasonic images. The computer deploys the zero-force drag method for the entire autonomous ultrasonic robot and controls the movement of the robotic arm to control the trajectory of the ultrasonic probe. It is worth noting that... Figure 1 There are two coordinate systems: the world coordinate system at the root of the robotic arm (including three axes: X0, Y0, and Z0), and the tool coordinate system at the end of the robotic arm (including three axes: X, Y, and Z). See also... Figure 2 The specific process of the method in this embodiment may include the following steps: Step S101: Based on the acquired rotation matrix and the first mechanical data corresponding to the three directions in the real-time acquired tool coordinate system, calculate the second mechanical data corresponding to the three directions in the world coordinate system. Step S102: Process each second mechanical data using a preset PID control algorithm to obtain the corresponding third mechanical data; Step S103: Based on the first preset value, preset parameters, each second mechanical data and the corresponding third mechanical data, calculate the displacement changes in the three directions, and combine the current positions in the three directions to obtain the positions in the three directions at the next moment. Step S104: Based on the preset boundary thresholds, current position, and next time position corresponding to the X and Z directions respectively, determine the corresponding minimum and maximum boundaries; Step S105: Obtain the preset minimum and maximum boundaries in the Y direction; Step S106: For the next time position in each direction, if it is less than the corresponding minimum boundary, update it to the corresponding minimum boundary; if it is greater than the corresponding maximum boundary, update it to the corresponding maximum boundary to obtain the predicted position. Step S107: Control the ultrasound probe to move to the predicted position in three directions.

[0025] To provide a clearer explanation of the above-mentioned zero-force dragging method for the autonomous ultrasonic robot, each step will be described in detail below.

[0026] In step S101 above, the first mechanical data includes force data values ​​and torque data values. The force data values ​​are used to calculate the position at the next moment, and the torque data values ​​are used to calculate the attitude at the next moment. Since the main purpose of this embodiment is to obtain the position at the next moment, the following steps will be described using force data values ​​as the first mechanical data.

[0027] Based on the first mechanical data (i.e. force data values) and rotation matrix corresponding to the X0, Y0, and Z0 directions in the tool coordinate system, the force data values ​​in the X, Y, and Z directions in the world coordinate system are calculated according to the following formula (1), thus obtaining the second mechanical data: In the above formula (1), Indicates the second mechanical data; Represents the rotation matrix; This represents the first mechanical data.

[0028] In existing joint space admittance control methods, sensor data (force / torque) is measured based on the tool coordinate system. When the robot arm's posture changes, the tool coordinate system rotates relative to the world coordinate system, causing a change in the direction component of the same physical force in the world coordinate system, thus affecting the spatial motion accuracy due to the robot arm's posture. In this embodiment, a rotation matrix is ​​used to transform the first mechanical data in the tool coordinate system to the second mechanical data in the world coordinate system, fundamentally eliminating the influence of robot arm posture changes on control commands and ensuring that the control reference remains stable in the world coordinate system, laying the foundation for subsequent precise control.

[0029] In step S102 above, the specific process of processing each second mechanical data using a preset PID control algorithm to obtain the corresponding third mechanical data may include the following steps: Step S1021: For each direction, the error amount is obtained based on the difference between the absolute value of the corresponding second mechanical data and the second preset value, wherein the absolute value of the second mechanical data is greater than the second preset value; In step S1021 above, the error amount Only in the active area ( (That is, the absolute value of the second mechanical data is greater than the second preset value) Definition: In the above formula (2), The second mechanical data can be the torque data value in three directions in the world coordinate system or the force data value in three directions in the world coordinate system; t represents the second preset value, which can be set to 2.5N for example. Indicates the amount of error.

[0030] when hour: (Positive direction error); when hour: (Negative directional error, absolute value is positive); when At this time: In the inactive region, the error amount is undefined (or set to...). (but not used for calculation).

[0031] Step S1022: Calculate the proportional gain based on the preset proportional gain reference value and error amount; In step S1022 above, the proportional gain reference value includes the minimum proportional gain, the maximum ratio, and the unity gain coefficient. Based on the minimum proportional gain, the maximum ratio, the unity gain coefficient, and the error, the proportional gain is calculated according to the following formula (3): In the above formula (3), Indicates proportional gain; Indicates the minimum proportional gain; Indicates the maximum ratio; Indicates the unity gain coefficient; Indicates the amount of error.

[0032] Furthermore, in the activated region, the squared error is calculated according to the following equation (4). Perform the calculation: In the inactive area No updates will be performed.

[0033] The squared error is used to evaluate the performance of PID control algorithms to reduce overshoot and suppress disturbances. For example, the integral of the squared error can be used as a performance evaluation index. When adjusting the parameters of the PID control algorithm (proportional, integral, and derivative terms), the integral of the squared error should be minimized to reduce the error and achieve good performance of the PID control algorithm.

[0034] Step S1023: Based on the product between the proportional gain and the error, the proportional term is obtained according to the following formula (5). : Step S1024: Based on the preset integral gain, error, and the obtained integral term from the previous moment, calculate the current integral term according to the following formula (6): In the above formula (6), This represents the current integral term, initially. Represents the integral term from the previous time step; This represents the integral gain.

[0035] Step S1025: Based on the second mechanical data, the second mechanical data obtained at the previous moment, and the preset differential gain, calculate the current differential term according to the following formula (7): In the above formula (7), Indicates the current differential term; Represents differential gain; This represents the second mechanical data, initially. This represents the second mechanical data from the previous moment.

[0036] Step S1026: Based on the proportional term, the current integral term, and the current differential term, calculate the output value according to the following formula (8). : Step S1027: If the second mechanical data is greater than the second preset value, the negative of the output value is used as the third mechanical data; if the second mechanical data is less than the second preset value, the output value is used as the third mechanical data. The expression is: In the above formula (9), when hour, Therefore ;when hour, Therefore ;when Furthermore, the integral term of the PID control algorithm is reset.

[0037] In this embodiment, the second mechanical data is optimized using a PID control algorithm to obtain the third mechanical data, so as to more stably and accurately track the operator's dragging intention and improve dynamic response and steady-state accuracy.

[0038] In step S103 above, the preset parameters include a first parameter and a second parameter. The specific process of obtaining the next-moment positions in the three directions may include the following steps: Step S1031: For each direction, if the corresponding second mechanical data is less than or equal to the first preset value, then the third mechanical data is used as the displacement change; otherwise, based on the third mechanical data, the second mechanical data, the first parameter, and the second parameter, the corresponding displacement change is calculated according to the following formula: In the above formula (10), This represents the change in displacement along the i-th direction; The third mechanical data represents the i-th direction; The second mechanical data in the i-direction is represented; A represents the first parameter, which can be set to 0.05 for example; B represents the second parameter, which can be set to 0.0005 for example.

[0039] Specifically, the change in displacement in the X direction It can be represented as: In the above formula (11), The third mechanical data representing the X-direction; The second mechanical data represents the X-direction; This represents the first preset value, which can be set to 5 for example.

[0040] displacement change in the Y direction It can be represented as: In the above formula (12), The third mechanical data represents the Y-direction; This represents the second mechanical data in the Y direction.

[0041] displacement change in the Z direction It can be represented as: In the above formula (13), The third mechanical data represents the Z-direction; The second mechanical data represents the Z-direction.

[0042] but This represents the change in displacement of the robotic arm after being subjected to force.

[0043] Step S1032: Summing the current positions and displacement changes in the three directions yields the next position in each direction. In the above formula (14), Indicates the position at the next moment; Indicates the current location.

[0044] In step S104 above, to ensure operational safety and motion stability during the robotic arm's dragging process, intelligent constraints must be implemented on the robotic arm's workspace. (See also...) Figure 1 As the ultrasonic probe held at the end of the robotic arm gradually approaches the human body, the extension range of the robotic arm increases, resulting in a significant reduction in the effective working space. To address this critical characteristic, this embodiment implements a funnel-shaped dynamic boundary constraint mechanism in the X-axis motion direction, based on the current position... and the next position The minimum and maximum boundaries of the robotic arm in the X direction are adjusted to achieve funnel-shaped motion constraints. Based on the actual situation, the boundary constraint mechanism in this embodiment mainly performs dynamic constraints on the robotic arm in the X and Z directions, while only performing basic constraint processing on the robotic arm in the Y direction.

[0045] The preset boundary thresholds in the X direction include a first minimum value, a first maximum value, and a boundary reference value; the preset boundary thresholds in the Z direction include a second minimum value, a second maximum value, and a height threshold. The specific process for determining the minimum and maximum boundaries corresponding to the X and Z directions may include the following steps: Step S1041: Based on the first minimum value, the first maximum value, and the boundary reference value, calculate the minimum boundary threshold and the maximum boundary threshold according to the following formula: In the above formula (15), Indicates the minimum boundary threshold; Indicates the maximum boundary threshold; This represents the first minimum value, which can be set to 0.4 for example; This represents the first maximum value, which can be set to 0.7 for example. This represents the boundary reference value, which can be set to 0.04 for example.

[0046] Step S1042: For the X direction, if the current position is less than or equal to the minimum boundary threshold and the position at the next moment is greater than the maximum boundary threshold, then the first minimum value is taken as the minimum boundary and the maximum boundary threshold is taken as the maximum boundary; if the current position is greater than or equal to the minimum boundary threshold and the position at the next moment is less than the minimum boundary threshold, then the minimum boundary threshold is taken as the minimum boundary and the first maximum value is taken as the maximum boundary; otherwise, the first minimum value is taken as the minimum boundary and the first maximum value is taken as the maximum boundary. In step S1042 above, during the specific execution of the X-direction constraint, a safety offset vector is used to dynamically constrain the X-direction based on the current and next positions of the robotic arm's end effector, determining the minimum and maximum safety distances. Finally, the minimum and maximum boundaries are determined based on these minimum and maximum safety distances. When dynamically constraining the X-direction, the minimum boundary is generally taken as the first minimum value, and the maximum boundary is generally taken as the first maximum value. However, the boundary values ​​in the X-direction will differ in the following two cases: (1) When the current position in the X direction is less than or equal to the minimum boundary threshold and the position at the next moment is greater than the maximum boundary threshold: In the above formula (16), Indicates the current position in the X direction; Indicates the position in the X direction at the next moment; This indicates an auxiliary item, which can be set to 0.0001 for example.

[0047] but: In the above formula (17), Indicates the maximum safe distance in the X direction; Indicates the minimum safe distance in the X direction; Indicates the minimum safe distance in the Z direction; Indicates the first minimum value; Indicates the second minimum value; Represents the maximum safe offset vector in the X direction; Represents the minimum safe offset vector in the X direction; This represents the minimum safe offset vector in the Z direction.

[0048] Then, the safety boundary (including the minimum boundary and the maximum boundary) is calculated using the following formula (18): In the above formula (18), Represents the minimum boundary in the X direction; Indicates the maximum boundary in the X direction; Represents the minimum boundary in the Y direction; Indicates the maximum boundary in the Y direction; This represents the minimum boundary in the Z direction.

[0049] Substituting equation (17) into equation (18), we obtain the minimum boundary in the X direction. Equal to the first minimum value Maximum boundary in the X direction Equal to the maximum boundary threshold Minimum boundary in the Z direction Equal to the second minimum value .

[0050] This case mainly involves dynamic constraints on the X direction, which can be summarized as follows: when the current position in the X direction is less than or equal to the minimum boundary threshold and the position at the next moment is greater than the maximum boundary threshold, the first minimum value is taken as the minimum boundary and the maximum boundary threshold is taken as the maximum boundary.

[0051] (2) When the current position in the X direction is greater than or equal to the minimum boundary threshold and the position at the next moment is less than the minimum boundary threshold: but In the above formula (20), This represents the first maximum value.

[0052] Substituting equation (20) into equation (18), we calculate the safety boundary: the minimum boundary in the X direction. equal to minimum boundary threshold Maximum boundary in the X direction Equal to the first maximum value Minimum boundary in the Z direction Equal to the second minimum value .

[0053] This case mainly involves dynamic constraints in the X direction, which can be summarized as follows: For the X direction, if the current position is greater than or equal to the minimum boundary threshold and the position at the next moment is less than the minimum boundary threshold, then the minimum boundary threshold is used as the minimum boundary and the first maximum value is used as the maximum boundary.

[0054] Step S1043: For the Z direction, if the corresponding current position is less than or equal to the height threshold, then the height threshold is used as the minimum boundary and the second maximum value is used as the maximum boundary; otherwise, the second minimum value is used as the minimum boundary and the second maximum value is used as the maximum boundary.

[0055] In step S1043 above, the main focus is on dynamically constraining the Z direction. Regardless of whether the current position is within the safe boundary range, the maximum boundary of the Z direction is preset to the second maximum value. The minimum boundary of the Z direction can be selected in the following ways: (1) The current height of the robotic arm in the Z direction is higher than a certain value: In the above formula (21), Indicates the current position in the Z direction; This represents the height threshold, which can be set to -0.2m for example.

[0056] but Substituting equation (22) into equation (18), we calculate the safety boundary: the minimum boundary in the X direction. Equal to the first minimum value Maximum boundary in the X direction Equal to the first maximum value Minimum boundary in the Z direction Equal to the second minimum value .

[0057] Therefore, it can be seen that when the height of the robotic arm in the Z direction is higher than the height threshold, the minimum boundary in the Z direction is always the second minimum value.

[0058] (2) When the current position in the X direction is less than the minimum boundary threshold and the current position in the Z direction is less than or equal to the height threshold: but Substituting equation (24) into equation (18), we calculate the safety boundary: the minimum boundary in the X direction. Equal to the first minimum value Maximum boundary in the X direction Equal to the first maximum value Minimum boundary in the Z direction Equal to height threshold .

[0059] This case mainly involves dynamic constraints in the Z direction, which can be summarized as follows: for the Z direction, if the corresponding current position is less than or equal to the height threshold, then the height threshold is used as the minimum boundary, and the second maximum value is used as the maximum boundary.

[0060] (3) When the current position in the X direction is greater than the maximum boundary threshold and the current position in the Z direction is less than or equal to the height threshold: but Substituting equation (26) into equation (18), we calculate the safety boundary: the minimum boundary in the X direction. Equal to the first minimum value Maximum boundary in the X direction Equal to the first maximum value Minimum boundary in the Z direction equal to height threshold .

[0061] This case mainly involves dynamic constraints in the Z direction, which can be summarized as follows: for the Z direction, if the corresponding current position is less than or equal to the height threshold, then the height threshold is used as the minimum boundary, and the second maximum value is used as the maximum boundary.

[0062] In step S106 above, it is necessary to determine the position at the next moment. If the position is within the safety boundary, then the robotic arm will execute the operation normally. Conversely, update. The value is as follows: If the X direction exceeds the boundary, then: If the Y direction exceeds the boundary, then: In the above formula (28), Indicates the current position in the Y direction; Represents the minimum boundary in the Y direction; This represents the maximum boundary in the Y direction.

[0063] If the Z direction exceeds the boundary, then: In the above formula (29), Indicates the current position in the Z direction; This represents the maximum boundary in the Z direction, i.e., the second maximum value, which can be set to 0.1 for example.

[0064] By summarizing equations (27)-(29) above, we can find that for each direction, if the corresponding next time step position is less than the corresponding minimum boundary, then the corresponding next time step position is updated to the corresponding minimum boundary; if the corresponding next time step position is greater than the corresponding maximum boundary, then the corresponding next time step position is updated to the corresponding maximum boundary. Based on the updated minimum boundary and the updated maximum boundary, the predicted position is obtained.

[0065] In this embodiment, the operator can manually drag the ultrasound probe at the end of the ultrasound robot to the initial scanning position under extremely low resistance (close to "zero force"). Simultaneously, this method considers the limitations of the robotic arm's working range and the safe distance between the robot and its surrounding environment (such as a enclosure). During the dragging process, this method dynamically calculates and adjusts the translational distance of the robotic arm in the X direction based on the real-time position of the ultrasound probe at the robot's end, and constrains the Y and Z directions. This aims to ensure that the robotic arm will not trigger motion limits or collide during the subsequent autonomous scanning phase, thus proactively avoiding the limitations of the robotic arm's workspace boundaries and the potential collision risks between the ultrasound robot and its surrounding environment (such as the patient bed, instrument cabinet, etc.).

[0066] In this embodiment, during data processing, the first mechanical data in the X0, Y0, and Z0 directions of the tool coordinate system is transformed into second mechanical data in the three directions of the world coordinate system using a rotation matrix. This allows for real-time data conversion to the world coordinate system, resolving control deviations caused by changes in the robotic arm's posture in existing joint space admittance control methods. The second mechanical data is optimized using a PID control algorithm to obtain third mechanical data. Combined with the first preset value, preset parameters, the second mechanical data, and the current position, the position at the next moment is calculated, precisely controlling the robotic arm to achieve zero-force drag and achieving coordinated optimization of motion smoothness and stability. Furthermore, due to physical limitations (boundaries, obstacles) in the robotic arm's workspace, collision prevention is necessary. This method uses a priori position correction based on dynamic constraints and safety boundaries. Dynamic constraints are applied to the X and Z directions, while a preset safety boundary is used in the Y direction. The method first determines whether the position at the next moment is within the safety boundary; if not, adjustments are made promptly, and then the ultrasonic probe is moved according to the predicted position. Compared to the a posteriori collision detection in existing joint space admittance control methods, this method predicts the safety of the next moment's position before the ultrasonic probe moves, fundamentally avoiding the risk of physical collisions of the robotic arm. It no longer relies on the speed of real-time detection and can effectively prevent the robotic arm from triggering motion limits or colliding, thus ensuring safety.

[0067] In another embodiment, the process of calculating the attitude at the next moment using the torque data value as the first mechanical data may include the following steps: Based on the preset torque rotation matrix and the first mechanical data (i.e. torque data values) in the X0, Y0, and Z0 directions of the tool coordinate system, the first mechanical data is converted into torque data values ​​in the three directions of the world coordinate system to obtain the second mechanical data. This step is consistent with the principle of step S101 above. Using the torque data values ​​in three directions in the world coordinate system as input, the above steps S1021-S1027 are executed using a PID control algorithm, and the output is the optimized torque value (i.e., the third mechanical data), which can be expressed as: ; Then construct the Euler angle rotation matrix. : The attitude at the next time step is calculated according to the following formula (31). : Using torque data as input, the attitude at the next moment is calculated. The physical torque applied by the operator is directly and with minimal delay converted into control commands to achieve more precise attitude control.

[0068] Example 2 Based on the same inventive concept, see [reference] Figure 3 This application also proposes a zero-force dragging device for an autonomous ultrasonic robot, comprising: The first data processing module 101 is used to calculate the second mechanical data corresponding to the three directions in the world coordinate system based on the acquired rotation matrix and the first mechanical data corresponding to the three directions in the real-time acquired tool coordinate system. The second data processing module 102 is used to process each second mechanical data using a preset PID control algorithm to obtain the corresponding third mechanical data. The position determination module 103 is used to calculate the displacement changes in three directions based on the first preset value, preset parameters, each second mechanical data and the corresponding third mechanical data, and to obtain the next position in the three directions by combining the current position in the three directions. The first boundary determination module 104 is used to determine the corresponding minimum boundary and maximum boundary based on the preset boundary thresholds corresponding to the X direction and Z direction, the current position and the next time position, respectively. The second boundary determination module 105 is used to obtain the preset minimum boundary and maximum boundary in the Y direction; The data update module 106 is used to update the position of the next time step in each direction to the corresponding minimum boundary if it is less than the corresponding minimum boundary, and to update it to the corresponding maximum boundary if it is greater than the corresponding maximum boundary, so as to obtain the predicted position. The control module 107 is used to control the movement of the ultrasound probe to the predicted position in three directions.

[0069] The autonomous ultrasonic robot zero-force dragging device provided in this embodiment of the invention has a similar implementation principle and technical effect to that of Embodiment 1, and will not be repeated here.

[0070] Example 3 Based on the same inventive concept, this application also proposes a computer-readable storage medium storing a computer program that, when executed by a processor, implements the zero-force dragging method for an autonomous ultrasonic robot as described in Embodiment 1.

[0071] The computer-readable storage medium may be included in the device / apparatus described in the above embodiments; or it may exist independently and not assembled into the device / apparatus. The computer-readable storage medium carries one or more programs, which, when executed, implement the method according to Embodiment 1 of the present invention.

[0072] According to embodiments of the present invention, the computer-readable storage medium may be a non-volatile computer-readable storage medium, such as including, but not limited to: portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In the present invention, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0073] Example 4 Based on the same inventive concept, this application also proposes a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the zero-force dragging method of the autonomous ultrasonic robot as described in Embodiment 1.

[0074] Example 5 Based on the same inventive concept, this application proposes a computer program product containing instructions that, when run on a computer device, cause the computer device to execute the zero-force dragging method of the autonomous ultrasonic robot in Embodiment 1.

[0075] The principles by which the above-mentioned devices, clients, media, and related equipment in this embodiment of the invention solve the problem are similar to those of the aforementioned methods. Therefore, their implementation can refer to the implementation of the aforementioned methods, and repeated details will not be repeated.

[0076] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage and optical storage) containing computer-usable program code.

[0077] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxesFigure 1 A device that provides the functions specified in one or more boxes.

[0078] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0079] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0080] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. This disclosure is not limited to the precise structures described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of this disclosure is limited only by the appended claims. Thus, if these modifications and variations of the invention fall within the scope of the claims of the invention and their equivalents, the invention is also intended to include these modifications and variations.

Claims

1. A zero-force dragging method for an autonomous ultrasonic robot, characterized in that, include: Based on the acquired rotation matrix and the first mechanical data corresponding to the three directions in the real-time tool coordinate system, the second mechanical data corresponding to the three directions in the world coordinate system are calculated. Each second mechanical data point is processed using a preset PID control algorithm to obtain the corresponding third mechanical data. Based on the first preset value, preset parameters, each of the second mechanical data and the corresponding third mechanical data, the displacement changes in the three directions are calculated, and combined with the current positions in the three directions, the positions in the three directions at the next moment are obtained. Based on the preset boundary thresholds corresponding to the X and Z directions, the current position, and the next time position, the corresponding minimum and maximum boundaries are determined. Obtain the preset minimum and maximum boundaries in the Y direction; For the next time step position in each direction, if it is less than the corresponding minimum boundary, it is updated to the corresponding minimum boundary; if it is greater than the corresponding maximum boundary, it is updated to the corresponding maximum boundary, thus obtaining the predicted position. Control the ultrasound probe to move to the predicted position in three directions.

2. The zero-force dragging method for an autonomous ultrasonic robot according to claim 1, characterized in that, The process of using a preset PID control algorithm to process each of the second mechanical data to obtain the corresponding third mechanical data includes: For each direction, the error is obtained based on the difference between the absolute value of the corresponding second mechanical data and the second preset value, wherein the absolute value of the second mechanical data is greater than the second preset value; The proportional gain is calculated based on the preset proportional gain reference value and the error amount; The proportional term is obtained based on the product of the proportional gain and the error amount; The current integral term is calculated based on the preset integral gain, the error amount, and the integral term obtained from the previous moment. Based on the second mechanical data, the second mechanical data obtained at the previous moment, and the preset differential gain, the current differential term is calculated. The output value is calculated based on the proportional term, the current integral term, and the current differential term; If the second mechanical data is greater than the second preset value, then the negative of the output value is used as the third mechanical data; if the second mechanical data is less than the second preset value, then the output value is used as the third mechanical data.

3. The zero-force dragging method for an autonomous ultrasonic robot according to claim 2, characterized in that, The proportional gain reference values ​​include the minimum proportional gain, the maximum ratio, and the unity gain coefficient; The calculation of the proportional gain based on the preset proportional gain reference value and the error amount includes: Based on the minimum proportional gain, the maximum ratio, the unity gain coefficient, and the error, the proportional gain is calculated according to the following formula: in, Indicates proportional gain; Indicates the minimum proportional gain; Indicates the maximum ratio; Indicates the unity gain coefficient; This indicates the amount of error.

4. The zero-force dragging method for an autonomous ultrasonic robot according to claim 1, characterized in that, The preset boundary thresholds in the X direction include a first minimum value, a first maximum value, and a boundary reference value; The preset boundary thresholds in the Z direction include a second minimum value, a second maximum value, and a height threshold; The determination of the corresponding minimum and maximum boundaries based on the preset boundary thresholds corresponding to the X and Z directions, the current position, and the next time-step position includes: Based on the first minimum value, the first maximum value, and the boundary reference value, the minimum boundary threshold and the maximum boundary threshold are calculated. For the X direction, if the corresponding current position is less than or equal to the minimum boundary threshold and the next position is greater than the maximum boundary threshold, then the first minimum value is taken as the minimum boundary and the maximum boundary threshold is taken as the maximum boundary; if the corresponding current position is greater than or equal to the minimum boundary threshold and the next position is less than the minimum boundary threshold, then the minimum boundary threshold is taken as the minimum boundary and the first maximum value is taken as the maximum boundary; otherwise, the first minimum value is taken as the minimum boundary and the first maximum value is taken as the maximum boundary. For the Z direction, if the corresponding current position is less than or equal to the height threshold, then the height threshold is used as the minimum boundary and the second maximum value is used as the maximum boundary; otherwise, the second minimum value is used as the minimum boundary and the second maximum value is used as the maximum boundary.

5. The zero-force dragging method for an autonomous ultrasonic robot according to claim 4, characterized in that, The calculation of the minimum boundary threshold and the maximum boundary threshold based on the first minimum value, the first maximum value, and the boundary reference value includes: Based on the first minimum value, the first maximum value, and the boundary reference value, the minimum boundary threshold and the maximum boundary threshold are calculated according to the following formula: in, Indicates the minimum boundary threshold; Indicates the maximum boundary threshold; Indicates the first minimum value; Indicates the first maximum value; Indicates the boundary reference value.

6. The zero-force dragging method for an autonomous ultrasonic robot according to claim 1, characterized in that, The preset parameters include a first parameter and a second parameter; Based on the first preset value, preset parameters, each of the second mechanical data and the corresponding third mechanical data, the displacement changes in three directions are calculated, and combined with the current positions in the three directions, the next position in the three directions is obtained, including: For each direction, if the corresponding second mechanical data is less than or equal to the first preset value, then the third mechanical data is used as the displacement change; otherwise, based on the third mechanical data, the second mechanical data, the first parameter, and the second parameter, the corresponding displacement change is calculated according to the following formula: in, This represents the change in displacement along the i-th direction; The third mechanical data represents the i-th direction; The second mechanical data represents the i-direction; A represents the first parameter; B represents the second parameter; The positions in the three directions at the next moment are obtained by summing the current positions and displacement changes in the three directions.

7. A zero-force dragging device for an autonomous ultrasonic robot, characterized in that, include: The first data processing module is used to calculate the second mechanical data corresponding to the three directions in the world coordinate system based on the acquired rotation matrix and the first mechanical data corresponding to the three directions in the real-time acquired tool coordinate system. The second data processing module is used to process each of the second mechanical data using a preset PID control algorithm to obtain the corresponding third mechanical data. The position determination module is used to calculate the displacement changes in three directions based on a first preset value, preset parameters, each second mechanical data and the corresponding third mechanical data, and to obtain the next position in the three directions by combining the current position in the three directions. The first boundary determination module is used to determine the corresponding minimum boundary and maximum boundary based on the preset boundary thresholds corresponding to the X and Z directions respectively, the current position and the next time position; The second boundary determination module is used to obtain the preset minimum and maximum boundaries in the Y direction; The data update module is used to update the position of the next time step in each direction to the corresponding minimum boundary if it is less than the corresponding minimum boundary, and to update it to the corresponding maximum boundary if it is greater than the corresponding maximum boundary, thus obtaining the predicted position. The control module is used to control the movement of the ultrasound probe to the predicted position in three directions.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the zero-force dragging method for the autonomous ultrasonic robot as described in any one of claims 1-6.

9. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the zero-force dragging method for an autonomous ultrasonic robot as described in any one of claims 1-6.

10. A computer program product containing instructions that, when run on a computer device, causes the computer device to perform the zero-force dragging method for an autonomous ultrasonic robot as described in any one of claims 1-6.

Images