A motion interpolation method and system for ultrasound robot autonomous scanning

Abstract

The present application relates to the field of robot control and medical ultrasonic imaging technology, and particularly relates to a motion interpolation method and system for autonomous scanning of an ultrasonic robot, comprising: obtaining input parameters and calculating displacement variation and attitude variation; determining a motion mode according to the displacement variation and the attitude variation, and constructing a two-phase segmentation model; establishing displacement closed constraint conditions and attitude closed constraint conditions based on the two-phase segmentation model; jointly solving the displacement closed constraint conditions, the attitude closed constraint conditions, attitude angle acceleration inhibition constraints and velocity non-negative constraints, and selecting the maximum variable speed segment length satisfying all the constraint conditions as an optimization result; according to the optimization result, respectively calculating the interpolation increments of displacement and attitude in the variable speed segment and the constant speed segment and accumulating, and outputting a synchronous displacement-attitude interpolation sequence. The present application not only ensures the safe fitting of the probe and the human body, but also improves the stability of standard section search and acquisition.

Description

Technical Field

[0001] This invention relates to the fields of robot control and medical ultrasound imaging technology, and in particular to a motion interpolation method and system for autonomous ultrasound robot scanning. Background Technology

[0002] In recent years, with the rapid development of medical robots and intelligent ultrasound imaging technology, ultrasound robots have been increasingly applied to autonomous scanning tasks of areas such as the breast, thyroid, and heart. Ultrasound robots, through robotic arms equipped with ultrasound probes, automate the scanning of the human body surface, reducing the workload of doctors and improving examination efficiency and diagnostic consistency. During autonomous scanning, the ultrasound probe needs to move along a predetermined path while adjusting its posture to maintain alignment with the body surface normal, thereby maintaining constant contact pressure. This places demands on the coordinated movement of displacement and posture in the probe's motion control. Motion interpolation methods, as a key link connecting path planning and underlying servo control, directly affect the smoothness of probe movement, contact stability, and the quality of ultrasound image acquisition.

[0003] Most existing ultrasonic robot motion interpolation methods employ linear interpolation or fixed acceleration interpolation to generate the probe's motion trajectory. Chinese patent CN110865610A discloses a tool trajectory interpolation and velocity planning method based on machine tool vibration suppression. This method effectively suppresses vibrations at trajectory corners of CNC machine tools through smooth trajectory interpolation and continuous acceleration velocity curve calculation. However, this solution is geared towards CNC machine tool motion scenarios, primarily focusing on the continuous processing of trajectory point sequences and corner transition optimization. It is not suitable for applications involving synchronous interpolation of displacement and attitude between two points in ultrasonic robots, and it does not consider end-effector velocity boundary conditions and safety contact force constraints. Chinese patent CN111897216A discloses a multi-segment velocity planning and interpolation method. This method inserts transition segments between continuous motion segments and employs S-shaped acceleration / deceleration optimization, achieving smooth velocity transitions through multi-segment trajectory pre-reading and spline interpolation node design. However, this method relies on complex multi-segment trajectory preprocessing, making it difficult to achieve simple and efficient control when only interpolation of displacement and attitude between two points is required, while also satisfying end-effector velocity and safety contact conditions. More importantly, existing technologies typically employ a separate optimization strategy when dealing with displacement interpolation and attitude interpolation. This means that the velocity curve is obtained by interpolating the displacement direction first, and the angular velocity curve is obtained by interpolating the attitude angle independently. Although the two are output synchronously in the time series, they are independent of each other during the optimization process. This separate processing method has the following technical drawbacks: First, displacement interpolation and attitude interpolation each seek the optimal acceleration and deceleration segment length parameters, which may result in the attitude still accelerating or decelerating when the displacement reaches uniform motion, leading to uncoordinated motion. Second, when scanning the curved surface of the human body, the ultrasound probe needs to maintain alignment with the surface normal to maintain constant contact pressure. However, the existing solution does not consider the dynamic impact of displacement motion on attitude adjustment requirements. When the robot experiences significant pose fluctuations during acceleration, the probe is subjected to both inertial and contact forces. If the attitude is also rapidly adjusted at this time, it will cause drastic fluctuations in contact force, potentially causing the probe to detach from the skin or apply excessive pressure, severely affecting the quality of ultrasound images. The existing separate optimization scheme cannot establish a constraint relationship between displacement acceleration and attitude angular acceleration. Third, from the perspective of energy consumption and stability, independent optimization results in a lack of coordination mechanism between displacement acceleration and attitude angular acceleration, which may lead to uneven distribution of robot joint torques and increased overall energy consumption. At the same time, introducing attitude changes during acceleration will superimpose dynamic disturbances, reducing the stability of probe contact. Summary of the Invention

[0004] In view of this, the present invention proposes a motion interpolation method and system for autonomous scanning of ultrasonic robots to solve the problems of motion incoordination, attitude fluctuation during acceleration, and insufficient fitting stability caused by displacement and attitude separation optimization in the prior art.

[0005] The technical solution of this invention is implemented as follows: This invention provides a motion interpolation method for autonomous scanning by an ultrasonic robot, comprising the following steps:

[0006] S1. Obtain the input parameters required for interpolation, and calculate the displacement and attitude changes based on the input parameters;

[0007] S2. Determine the motion mode based on the displacement change and attitude change, and construct a two-phase segmented model based on the determined motion mode. The two-phase segmented model includes a variable speed segment and a constant speed segment, so that the displacement and attitude are executed synchronously in time. In the variable speed segment, the attitude angular acceleration is suppressed and constrained, and the attitude is compensated and adjusted in the constant speed segment.

[0008] S3. Based on the two-phase piecewise model, establish displacement closure constraints and attitude closure constraints. The displacement closure constraints ensure displacement hit by compensation at the end of the displacement. The attitude closure constraints are used to calculate the attitude compensation acceleration of the uniform speed segment and ensure the attitude is in place by attitude compensation of the uniform speed segment.

[0009] S4. Within the range of values ​​for the variable speed segment length parameter, jointly solve the displacement closure constraint, attitude closure constraint, attitude angular acceleration suppression constraint, and velocity non-negativity constraint, and select the maximum variable speed segment length that satisfies all constraints as the optimization result.

[0010] S5. Based on the optimization results, calculate and accumulate the interpolation increments of displacement and attitude in the variable speed and constant speed segments respectively, and output a synchronized displacement-attitude interpolation sequence.

[0011] Based on the above technical solutions, preferably, in step S1, the input parameters include the initial position of the end effector, the target position, the initial velocity in the displacement direction, the initial attitude, the target attitude, the initial angular velocity in the attitude direction, the maximum allowable value of the attitude angular acceleration, and the total number of interpolation steps.

[0012] Based on the above technical solutions, preferably, step S2 specifically includes:

[0013] S21. Calculate the average velocity in the displacement direction based on the displacement change, and calculate the average angular velocity in the attitude direction based on the attitude change.

[0014] S22. Determine the motion mode of displacement based on the relationship between the average velocity in the displacement direction and the initial velocity, and determine the motion mode of attitude based on the relationship between the average angular velocity in the attitude direction and the initial angular velocity. The motion modes include the acceleration-then-uniform-speed mode and the deceleration-then-uniform-speed mode.

[0015] S23. Based on the determined motion mode, construct the basic framework of a two-phase segmented model in which displacement and attitude share the length parameter of the variable speed segment. Displacement and attitude are executed synchronously in time sequence. The basic framework sets the speed to change linearly in the variable speed segment and keeps the speed constant in the uniform speed segment.

[0016] S24. Apply angular acceleration suppression constraints to the attitude direction in the speed change section of the two-phase segmented basic frame. The suppression constraints limit the attitude angular acceleration to not exceed the product of the attenuation coefficient and the maximum allowable value of the attitude angular acceleration.

[0017] S25. In the uniform velocity segment of the two-phase segmented basic framework, a compensation acceleration is set for the attitude direction. The compensation acceleration is used to complete the attitude adjustment, and the absolute value of the compensation acceleration does not exceed the maximum allowable value of the attitude angular acceleration, thus forming a complete two-phase segmented model.

[0018] Based on the above technical solutions, the preferred expression for the two-phase piecewise model of displacement direction and velocity is as follows:

[0019] ;

[0020] For the attitude direction, the two-phase piecewise model expression for angular velocity is:

[0021] ;

[0022] in, This is the current step size number. N is the total number of interpolation steps; The parameter for the length of the gear shift segment indicates the number of steps in the shift segment. When it is in the shifting section, when The time is a period of uniform speed; The initial velocity is in the direction of displacement. For displacement acceleration, The average velocity in the direction of displacement. The initial angular velocity in the attitude direction, The attitude angular acceleration during the speed change segment satisfies the constraints. , The attenuation coefficient is... This represents the maximum permissible value for attitude angular acceleration. For the attitude compensation acceleration during the uniform velocity segment, the constraints must be satisfied. .

[0023] Based on the above technical solutions, preferably, step S3 specifically includes:

[0024] S31. Calculate the cumulative displacement in the displacement direction and the cumulative attitude change in the attitude direction within the speed change range based on the two-phase segmented model.

[0025] S32. Obtain the remaining displacement of the constant speed segment based on the difference between the magnitude of the displacement change and the cumulative displacement of the variable speed segment; obtain the remaining attitude change of the constant speed segment based on the difference between the magnitude of the attitude change and the cumulative attitude change of the variable speed segment.

[0026] S33. Calculate the theoretical number of steps in the uniform segment based on the remaining displacement and the average velocity in the displacement direction. Take the integer part of the theoretical number of steps in the uniform segment as the actual number of steps in the uniform segment. At the end of the displacement, compensate for the displacement corresponding to the fractional part of the theoretical number of steps in the uniform segment and establish displacement closure constraint conditions.

[0027] S34. Establish attitude closure constraints, and solve for the attitude compensation acceleration of the uniform speed segment based on the attitude closure constraints and the expression for the cumulative attitude change of the uniform speed segment.

[0028] Based on the above technical solutions, the preferred embodiment is characterized by:

[0029] The expression for the displacement closure constraint is:

[0030] ;

[0031] in, This represents the cumulative displacement during the speed change phase. The average velocity in the direction of displacement; The actual number of steps in the uniform speed segment is denoted as , and the integer part of the theoretical number of steps in the uniform speed segment is denoted as . The decimal part of the theoretical number of steps in the uniform speed segment. ; The magnitude of the displacement change; The amount of displacement compensated at the end of the displacement;

[0032] The expression for the attitude closure constraint is:

[0033] ;

[0034] in, This represents the cumulative attitude change during the speed change phase. This represents the cumulative attitude change during the constant velocity segment. The magnitude of the attitude change;

[0035] The expression for attitude compensation acceleration during the uniform velocity segment is:

[0036] ;

[0037] in, For the attitude compensation acceleration during the uniform velocity segment, the constraints must be satisfied. ; The modulus of the remaining attitude change during the uniform velocity segment. ; The initial angular velocity of the uniform velocity segment, ; The initial angular velocity; This refers to the attitude angular acceleration during the speed change phase. This is the parameter for the length of the speed change section; This represents the total number of interpolation steps. This represents the maximum permissible value for attitude angular acceleration.

[0038] Based on the above technical solutions, preferably, step S4 specifically includes:

[0039] The range of values ​​for the variable speed section length parameter is determined as follows: ,in Determined by the minimum acceleration / deceleration ratio, This represents the total number of interpolation steps.

[0040] Within the range of values, candidate values ​​are iterated and optimized. For each candidate value, the displacement acceleration is calculated according to the displacement closure constraint and it is checked whether the cumulative displacement of the variable segment exceeds the magnitude of the displacement change. The attitude compensation acceleration of the uniform segment is calculated according to the attitude closure constraint and the attitude angular acceleration suppression constraint of the variable segment, and its absolute value is checked whether it does not exceed the maximum allowable value of attitude angular acceleration. The velocity non-negativity constraint is checked, and the maximum variable segment length is selected as the optimization result from the candidate values ​​that satisfy all constraints.

[0041] Based on the above technical solutions, the preferred expression for displacement acceleration is:

[0042] ;

[0043] in, It is displacement acceleration; The magnitude of the displacement change; The initial velocity;

[0044] The expression for the cumulative attitude change during the speed change phase is:

[0045] ;

[0046] in, This represents the maximum attitude change that can be achieved during the speed change segment. The initial angular velocity; The attenuation coefficient; This represents the maximum permissible value for attitude angular acceleration.

[0047] Based on the above technical solutions, preferably, step S5 specifically includes:

[0048] S51. Based on the length of the variable speed section determined by the optimization results, calculate the interpolation increment of the displacement direction and attitude direction at each step within the variable speed section.

[0049] S52. Calculate the interpolation increment of displacement direction and attitude direction for each step in the uniform velocity segment, where the attitude direction is adjusted using the attitude compensation acceleration of the uniform velocity segment.

[0050] S53. The interpolation increments in the displacement direction are distributed according to the normalized direction vector of the displacement change, and accumulated to obtain the displacement interpolation sequence of each coordinate axis in three-dimensional space.

[0051] S54. The interpolation increments in the attitude direction are distributed according to the normalized direction vector of the attitude change, and the interpolation sequences of the three attitude angles are accumulated.

[0052] S55. Combine the displacement interpolation sequence and the attitude interpolation sequence to output a synchronized displacement-attitude interpolation sequence.

[0053] The present invention also provides a motion interpolation system for autonomous scanning by an ultrasonic robot, characterized in that it includes a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the motion interpolation method as described above.

[0054] The motion interpolation method and system for autonomous ultrasonic robot scanning of the present invention have the following advantages over the prior art:

[0055] (1) By constructing a two-phase piecewise model with shared temporal parameters for displacement and attitude, suppression constraints are applied to the attitude angular acceleration in the variable speed segment, and attitude is compensated and adjusted in the uniform speed segment. Displacement closure constraints and attitude closure constraints are established, and multiple constraints are jointly solved to optimize the length parameters of the variable speed segment, thus achieving deep coupling between displacement motion state and attitude adjustment requirements. Compared with the existing scheme of separate optimization of displacement interpolation and attitude interpolation, this invention ensures that the acceleration stage and uniform speed stage of displacement and attitude are completely synchronized in time, avoiding the incoordination phenomenon that the displacement has entered uniform speed while the attitude is still accelerating. It effectively reduces the contact force fluctuation caused by the mismatch of motion state, significantly improves the stability of the ultrasonic probe's contact with the body surface and the quality of ultrasonic image acquisition, and at the same time ensures that the displacement accurately hits the target position and the attitude accurately reaches the target attitude.

[0056] (2) By constructing a two-phase piecewise model, the velocity is ensured to be continuous, but there is a step acceleration at the junction of the acceleration segment and the uniform velocity segment. Compared with the S-curve, the continuity of acceleration is sacrificed, but the calculation is simplified and real-time. At the same time, the computational load is kept very low, which can be run quickly in the real-time control system of the ultrasound robot. This ensures the safe contact between the probe and the human body and improves the stability of standard section search and acquisition, providing a more advanced and reliable motion control scheme for the clinical application of ultrasound robots. Attached Figure Description

[0057] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0058] Figure 1 This is a flowchart of the motion interpolation method for autonomous scanning by an ultrasonic robot according to the present invention;

[0059] Figure 2 This is a schematic diagram of the velocity curve of the two-phase segmented model of the present invention;

[0060] Figure 3 This is a schematic diagram comparing the traditional separation optimization method with the method of this invention. Detailed Implementation

[0061] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0062] like Figure 1 As shown, the present invention provides a motion interpolation method for autonomous scanning by an ultrasonic robot, comprising the following steps:

[0063] S1. Obtain the input parameters required for interpolation, and calculate the displacement and attitude changes based on the input parameters;

[0064] S2. Determine the motion mode based on the displacement change and attitude change, and construct a two-phase segmented model based on the determined motion mode. The two-phase segmented model includes a variable speed segment and a constant speed segment, so that the displacement and attitude are executed synchronously in time. In the variable speed segment, the attitude angular acceleration is suppressed and constrained, and the attitude is compensated and adjusted in the constant speed segment.

[0065] S3. Based on the two-phase piecewise model, establish displacement closure constraints and attitude closure constraints. The displacement closure constraints ensure displacement hit by compensation at the end of the displacement. The attitude closure constraints are used to calculate the attitude compensation acceleration of the uniform speed segment and ensure the attitude is in place by attitude compensation of the uniform speed segment.

[0066] S4. Within the range of values ​​for the variable speed segment length parameter, jointly solve the displacement closure constraint, attitude closure constraint, attitude angular acceleration suppression constraint, and velocity non-negativity constraint, and select the maximum variable speed segment length that satisfies all constraints as the optimization result.

[0067] S5. Based on the optimization results, calculate and accumulate the interpolation increments of displacement and attitude in the variable speed and constant speed segments respectively, and output a synchronized displacement-attitude interpolation sequence.

[0068] This invention constructs a hybrid two-phase piecewise model in which displacement and attitude share the acceleration segment length parameter. It introduces the suppression constraint of displacement acceleration on attitude angular acceleration and a phased attitude adjustment strategy. While ensuring strict displacement closure, precise attitude positioning, and continuous and smooth velocity curve, it achieves deep coupling between displacement motion state and attitude adjustment requirements. This ensures that the ultrasonic robot probe has stable attitude during acceleration segment, smooth attitude adjustment during uniform speed segment, stable fit, and reliable imaging during scanning.

[0069] In one specific embodiment of the present invention, in step S1, the input parameters required for interpolation are first obtained. These parameters typically originate from the upper-level path planning module, such as path points planned using a 3D point cloud. The input parameters include the initial position of the end effector. Target location Initial velocity in the direction of displacement Initial attitude angle Target attitude angle Initial angular velocity Attitude direction, maximum allowable value of attitude angular acceleration and total interpolation steps .in, and In three-dimensional space coordinates, and These represent the initial attitude angle and target attitude angle of the end effector, including roll angle, pitch angle, and yaw angle, respectively. Used to limit the intensity of posture changes to ensure the stability of the ultrasound probe's contact with the body surface. Used to limit the time length of the interpolation process. Calculates the displacement change based on the input parameters. and attitude change These input parameters, along with the calculated displacement and attitude changes, are used as boundary conditions for subsequent interpolation calculations to ensure that the comprehensive requirements of non-negative velocity, strictly hit displacement, accurate positioning attitude, and stable attitude during acceleration are met within a finite number of steps.

[0070] In a specific embodiment of the present invention, step S2 specifically includes:

[0071] S21. Calculate the average velocity in the displacement direction based on the displacement change, and calculate the average angular velocity in the attitude direction based on the attitude change. Specifically, the formula for calculating the average velocity in the displacement direction is: ,in The magnitude of the displacement change This represents the total number of interpolation steps. This represents the average velocity required to complete the displacement change within the total number of interpolation steps. The formula for calculating the average angular velocity in the attitude direction is: ,in The magnitude of the attitude change. This represents the average angular velocity required to complete the attitude change within the total number of interpolation steps.

[0072] S22. Determine the motion mode of displacement based on the relationship between the average velocity in the displacement direction and the initial velocity, and determine the motion mode of attitude based on the relationship between the average angular velocity in the attitude direction and the initial angular velocity. The motion modes include the acceleration-then-uniform-speed mode and the deceleration-then-uniform-speed mode.

[0073] Specifically, regarding the direction of displacement, if If the displacement adopts an acceleration-then-constant speed mode, it means that the end effector needs to accelerate from a lower initial speed to a constant speed; if If the displacement follows a deceleration-then-uniform-speed pattern, it means the end effector needs to decelerate from a higher initial velocity to a constant speed. For the attitude direction, if... The attitude adopts an acceleration-then-constant-speed mode; if If the motion pattern is such that the attitude first decelerates and then becomes uniform, the determination of the motion pattern provides sign information of acceleration for the subsequent construction of the two-phase piecewise model.

[0074] S23. Based on the determined motion pattern, construct the basic framework of a two-phase piecewise model that shares the variable speed segment length parameter for displacement and attitude. Displacement and attitude are executed synchronously in time. The two-phase piecewise model divides the entire interpolation process into two stages: in the variable speed segment, the velocity is set to change linearly, and in the uniform speed segment, the velocity is set to remain constant. By introducing a unified variable speed segment length parameter, displacement interpolation and attitude interpolation share the same variable speed segment length, thereby establishing temporal coupling between displacement and attitude during the optimization process. The synchronous execution of displacement and attitude in time ensures that their acceleration and uniform speed stages are completely synchronized in time, avoiding the inconsistency phenomenon where displacement has entered uniform speed while attitude is still accelerating, thus ensuring the continuity of the velocity curve.

[0075] S24. Apply angular acceleration suppression constraints to the attitude direction in the speed change section of the two-phase segmented basic frame. The suppression constraints limit the attitude angular acceleration to not exceed the product of the attenuation coefficient and the maximum allowable value of the attitude angular acceleration.

[0076] Specifically, the purpose of suppression constraints is to ensure relatively stable attitude during the acceleration phase, avoiding significant attitude adjustments during the complex acceleration stage where the probe is subjected to stress. Suppression constraints are achieved by limiting the attitude angular acceleration to the product of the attenuation coefficient and the maximum allowable value of the attitude angular acceleration. ,in This refers to the attitude angular acceleration during the speed change phase. The attenuation coefficient is... , This represents the maximum permissible value for attitude angular acceleration. More preferably, the typical range of the attenuation coefficient is 0.3-0.5, with the specific value selected based on the application scenario: a smaller value can be used for soft tissue scans such as breast scans to ensure higher contact stability, while a larger value can be used for scenarios requiring rapid response, such as cardiac scans, to balance attitude adjustment efficiency. By applying a suppressive constraint to the attitude angular acceleration during the variable speed range, the fluctuation of contact force caused by rapid attitude changes is effectively reduced, improving the stability of the ultrasound probe's contact with the body surface.

[0077] S25. In the uniform velocity segment of the two-phase segmented basic framework, a compensation acceleration is set for the attitude direction. The compensation acceleration is used to complete the attitude adjustment, and the absolute value of the compensation acceleration does not exceed the maximum allowable value of the attitude angular acceleration, thus forming a complete two-phase segmented model.

[0078] Specifically, because the uniform velocity phase of the displacement motion is smooth, this is the optimal time to adjust the attitude. The attitude can be adjusted significantly to quickly reach the target attitude. The absolute value of the compensation acceleration should not exceed the maximum allowable value of the attitude angular acceleration, i.e. ,in This involves setting an attitude compensation acceleration for the constant velocity segment. By assigning a compensation acceleration to the constant velocity segment, the main attitude adjustment task is distributed to the smooth, constant velocity segment, thus realizing a staged attitude adjustment strategy. This ensures accurate attitude positioning and improves the stability of probe contact.

[0079] Based on the steps described above, the complete two-phase piecewise model constructed has the following expression for the displacement direction and velocity:

[0080] ;

[0081] For the attitude direction, the two-phase piecewise model expression for angular velocity is:

[0082] ;

[0083] in, This is the current step size number. N is the total number of interpolation steps; The parameter for the length of the gear shift segment indicates the number of steps in the shift segment. When it is in the shifting section, when The time is a period of uniform speed; The initial velocity is in the direction of displacement. For displacement acceleration, The average velocity in the direction of displacement. The initial angular velocity in the attitude direction, The attitude angular acceleration during the speed change segment satisfies the constraints. , The attenuation coefficient is... This represents the maximum permissible value for attitude angular acceleration. For the attitude compensation acceleration during the uniform velocity segment, the constraints must be satisfied. .

[0084] In this invention, by sharing the same variable-speed segment length parameter for displacement and attitude, the acceleration and constant-speed phases of both are ensured to be completely synchronized in time. By applying suppression constraints to the attitude angular acceleration during the variable-speed segment, large-scale attitude adjustments are avoided during the acceleration phase when the probe is under complex forces. Instead, the main attitude adjustment task is allocated to the stable constant-speed segment, thereby significantly improving the stability of probe contact and the quality of ultrasound image acquisition. Figure 2A schematic diagram of the velocity curves of the two-phase segmented model is shown. As can be seen from the figure, the velocity curves in the displacement direction and the angular velocity curves in the attitude direction share the variable speed segment length parameter k. In the variable speed segment, the displacement velocity increases linearly and the acceleration is constant, while the attitude angular velocity is suppressed and constrained, and its angular acceleration is limited to a small range to avoid drastic attitude changes during the acceleration phase. In the uniform speed segment, the displacement maintains stable motion, while the attitude is quickly adjusted to the target attitude through a larger compensation acceleration, ensuring that the displacement hits the target accurately and the attitude is in place, thus achieving deep coupling and temporal synchronization between the motion state and the attitude adjustment.

[0085] In a specific embodiment of the present invention, step S3 specifically includes:

[0086] S31. Calculate the cumulative displacement in the displacement direction and the cumulative attitude change in the attitude direction within the speed change range based on the two-phase segmented model.

[0087] Regarding the direction of displacement, according to the speed expression for the variable speed segment... ,in The cumulative displacement is obtained by summing the speeds at each step within the speed change range. The formula for calculating the cumulative displacement is as follows:

[0088] ;

[0089] in This represents the cumulative displacement during the speed change phase. This is the parameter for the length of the speed change section. The initial velocity is in the direction of displacement. This represents displacement acceleration.

[0090] Regarding the attitude direction, according to the expression for the angular velocity of the variable speed segment... ,in The cumulative attitude change is obtained by summing the angular velocities at each step within the speed change segment. The formula for calculating the cumulative attitude change is as follows:

[0091] ;

[0092] in This represents the cumulative attitude change during the speed change phase. The initial angular velocity in the attitude direction, The attitude angular acceleration during the speed change segment satisfies the suppression constraint. .

[0093] S32. Obtain the remaining displacement of the constant speed segment based on the difference between the magnitude of the displacement change and the cumulative displacement of the variable speed segment; obtain the remaining attitude change of the constant speed segment based on the difference between the magnitude of the attitude change and the cumulative attitude change of the variable speed segment.

[0094] The formula for calculating the remaining displacement in the uniform velocity segment is: ,in This represents the remaining displacement during the uniform velocity segment. The magnitude of the displacement change This represents the cumulative displacement during the variable speed phase. The formula for calculating the remaining attitude change during the uniform speed phase is: ,in This represents the remaining attitude change during the uniform velocity segment. The magnitude of the attitude change. This represents the cumulative attitude change during the acceleration phase. Because a restraining constraint is applied to the attitude angular acceleration during the acceleration phase, the acceleration phase typically cannot complete all attitude changes. The remaining attitude changes need to be compensated and adjusted during the smooth, constant-speed phase. It is usually a positive value and may account for a large proportion.

[0095] S33. Calculate the theoretical number of steps in the uniform segment based on the remaining displacement and the average velocity in the displacement direction. Take the integer part of the theoretical number of steps in the uniform segment as the actual number of steps in the uniform segment, and compensate for the displacement corresponding to the decimal part of the theoretical number of steps in the uniform segment at the end of the displacement to establish displacement closure constraint conditions.

[0096] The formula for calculating the theoretical number of steps in the uniform speed segment is as follows:

[0097] ;

[0098] in This represents the remaining displacement during the uniform velocity segment. The average velocity in the direction of displacement. This represents the integer part of the theoretical number of steps in the uniform speed segment, i.e., the actual number of steps in the uniform speed segment. The decimal part of the theoretical number of steps in the uniform speed segment. .

[0099] In the discrete interpolation process, the uniform segment can only perform an integer number of steps. decimal part Corresponding displacement Since this cannot be achieved through a complete uniform speed step, compensation is required at the end of the displacement, i.e., the final step. By compensating at the end of the displacement, it is ensured that the sum of the cumulative displacement of the variable speed segment, the cumulative displacement of the uniform speed segment, and the compensation displacement at the end is strictly equal to the modulus of the target displacement change, thereby establishing a displacement closure constraint condition.

[0100] The expression for the displacement closure constraint condition is:

[0101] ;

[0102] in This represents the cumulative displacement during the speed change phase. This is the amount of displacement compensated at the end of the displacement. This constraint ensures that, under discrete sampling conditions, the displacement strictly hits the target position with zero displacement error.

[0103] S34. Establish attitude closure constraints, and solve for the attitude compensation acceleration of the uniform speed segment based on the attitude closure constraints and the expression for the cumulative attitude change of the uniform speed segment.

[0104] The attitude closure constraint requires that the sum of the cumulative attitude changes during the variable speed phase and the cumulative attitude changes during the constant speed phase equals the magnitude of the attitude change, expressed as follows:

[0105] ;

[0106] in This represents the cumulative attitude change during the speed change phase. This represents the cumulative attitude change during the constant velocity phase. Let be the magnitude of the attitude change.

[0107] During the constant velocity phase, attitude is adjusted using compensated acceleration, and its angular velocity expression is:

[0108] ;

[0109] in , The initial angular velocity of the uniform velocity segment, , This refers to the attitude compensation acceleration during the uniform velocity segment. The cumulative attitude change during the uniform velocity segment is obtained by summing the angular velocities at each step within that segment; its expression is:

[0110] ;

[0111] in This represents the total number of interpolation steps. Let be the number of steps in the uniform velocity segment. Substituting the expression for the cumulative attitude change in the uniform velocity segment into the attitude closure constraint conditions, and simplifying, the attitude compensation acceleration in the uniform velocity segment can be solved. The expression for the attitude compensation acceleration in the uniform velocity segment is:

[0112] ;

[0113] in The modulus of the remaining attitude change during the uniform velocity segment. , This refers to the attitude angular acceleration during the speed change phase. This is the parameter for the length of the speed change section. This represents the total number of interpolation steps. The calculated attitude compensation acceleration for the uniform velocity segment must satisfy the constraints. ,in This represents the maximum permissible value of attitude angular acceleration. This constraint ensures that the attitude compensation acceleration during the constant velocity phase does not exceed the maximum permissible angular acceleration of the system, thereby guaranteeing the safety and stability of attitude adjustment.

[0114] By establishing displacement closure constraints and attitude closure constraints, precise closure of displacement and attitude within a finite total number of interpolation steps is ensured. Displacement closure achieves strict hit detection through an end-effector compensation mechanism, while attitude closure achieves precise positioning through a uniform acceleration compensation mechanism. The combined effect of these two closure constraints ensures that both displacement and attitude meet the stringent requirements of the boundary conditions during the optimization process.

[0115] In a specific embodiment of the present invention, in step S4, firstly, the length parameter of the transmission segment is determined. The range of values ​​is ,in This is the lower limit for the length of the gear shift interval, determined by engineering experience or the minimum acceleration / deceleration ratio. It is used to avoid excessive acceleration due to an excessively short acceleration / deceleration interval. Total number of interpolation steps, upper limit This ensures that at least one step is reserved for the uniform velocity segment or the final closing step. In practical applications, It can be set according to the system's dynamic performance and safety requirements, with a typical value of 10% to 20% of the total number of steps.

[0116] The candidate values ​​are iterated and optimized within the given range. The length of the variable speed segment for each candidate value is determined. Perform constraint checks according to the following steps. First, calculate the displacement acceleration based on the displacement closure constraint conditions. The expression for displacement acceleration is:

[0117] ;

[0118] in For the corresponding candidate value The formula is derived through displacement closure constraints, ensuring that, given a total number of steps and initial velocity, the acceleration during the variable speed segment can precisely close the displacement. Within the range of k... min From k within [N-1] min We begin by incrementally traversing the candidate value k, and calculate the displacement acceleration and the cumulative displacement during the acceleration / deceleration phase. To check whether the cumulative displacement of the transmission section exceeds the modulus of the displacement change, i.e., to check... Is it true? If so... This indicates that the candidate value This resulted in the acceleration phase exceeding the target displacement, with the remaining displacement being negative. If values ​​larger than k are not feasible, the traversal terminates; during the traversal, the maximum k value that satisfies all constraints is recorded as the optimal variable speed segment length parameter k. .

[0119] Secondly, the attitude compensation acceleration for the constant velocity segment is calculated based on the attitude closure constraint and the attitude angular acceleration suppression constraint for the variable speed segment, and its compliance with the constraint conditions is verified. Suppression constraints are applied to the attitude angular acceleration during the variable speed segment. Under the given conditions, calculate the maximum attitude change that can be achieved during the speed change segment. When the attitude angular acceleration during the speed change segment reaches its upper limit, the expression for the cumulative attitude change during the speed change segment is:

[0120] ;

[0121] in For the corresponding candidate value The maximum attitude change that the speed change segment can achieve. The initial angular velocity, The attenuation coefficient is... This represents the maximum permissible value for attitude angular acceleration.

[0122] Based on the modulus of attitude change Calculate the remaining attitude change during the uniform velocity segment The attitude compensation acceleration during the uniform velocity segment is obtained.

[0123] ;

[0124] Among them, when the attitude angular acceleration in the speed change segment takes the upper limit value γ a max At that time, the initial angular velocity of the uniform segment was Verify whether the compensated acceleration satisfies the constraints. .like This indicates the current situation. Even with the maximum angular acceleration during the uniform velocity segment, the attitude closure cannot be completed at the given value; therefore, this candidate value is not feasible and needs to be reduced. Alternatively, adjust the attitude angular acceleration during the acceleration phase.

[0125] In addition, the non-negativity constraint of velocity must be verified. For the displacement direction, the velocity in the uniform segment must be guaranteed. This means the average velocity in the displacement direction is non-negative. For the attitude direction, the angular velocity in each step within the uniform velocity segment must be non-negative or meet the motion mode requirements. When the end effector's ending velocity is positive, the sum of the acceleration and uniform velocity segments must also not exceed the total number of steps. ,Right now If the limit is exceeded, then it needs to be... Adjust downwards until the constraint is satisfied.

[0126] The longest variable speed segment length is selected from the candidate values ​​that satisfy all constraints as the optimization result, denoted as . The reason for choosing the maximum speed range length is that, for acceleration mode (i.e., the magnitude of displacement change), ), increase This can reduce the magnitude of displacement acceleration and make the velocity curve smoother; for deceleration mode ( A longer deceleration phase prevents excessively high end-effector velocity. This ensures that the remaining constant velocity phase is non-negative, achieves a smoother velocity curve under given boundary conditions, and maintains stable attitude during acceleration. The main adjustments are concentrated in the constant velocity phase, resulting in more stable overall operation. When the end-effector's velocity is zero, the constant velocity phase does not exist; the acceleration phase must completely cover all displacement and attitude changes. It is constrained by both the displacement closure equation and the attitude closure equation. If in There is no element within the range that satisfies all constraints. Value, i.e. constraint conflict, can adopt a degradation strategy. The following degradation strategies can be tried in sequence: (1) appropriately relax the attitude suppression coefficient γ of the acceleration segment; (2) appropriately increase the total number of interpolation steps N; (3) if the attitude adjustment amount is very small, it can degenerate into pure displacement interpolation mode.

[0127] In one specific embodiment of the present invention, in step S5,

[0128] S51. Based on the length of the variable speed segment determined by the optimization results, calculate the interpolation increment of the displacement direction and attitude direction at each step within the variable speed segment.

[0129] Regarding the direction of displacement, in the speed change section Inside, the displacement increment for each step is calculated based on the velocity expression.

[0130] ;

[0131] in This is the displacement acceleration calculated based on the displacement closure constraint condition.

[0132] Regarding attitude and direction, during the speed change phase Inside, the attitude change increment for each step is calculated based on the angular velocity expression.

[0133] ;

[0134] in The attitude angular acceleration during the speed change segment satisfies the suppression constraint. .

[0135] S52. Calculate the interpolation increment of the displacement direction and attitude direction at each step in the uniform velocity segment, where the attitude direction is adjusted using the attitude compensation acceleration of the uniform velocity segment.

[0136] Regarding the direction of displacement, in the uniform velocity segment Within, the displacement increment remains constant. ,in This represents the average velocity along the displacement direction. It's important to note that, according to the displacement closure constraint, at the end of the displacement phase (the final step), the decimal part of the theoretical number of steps for the uniform velocity segment needs to be adjusted. The corresponding displacement is compensated, and the displacement increment in the final step is:

[0137] ;

[0138] Regarding attitude direction, during the constant velocity segment Inside, the attitude direction is adjusted using compensated acceleration, and the angular velocity expression is:

[0139] ;

[0140] The attitude change increment is:

[0141] ;

[0142] in The initial angular velocity of the uniform velocity segment, This is the attitude compensation acceleration for the uniform velocity segment calculated based on the attitude closure constraint conditions.

[0143] S53. The interpolation increments in the displacement direction are distributed according to the normalized direction vector of the displacement change, and accumulated to obtain the displacement interpolation sequence of each coordinate axis in three-dimensional space.

[0144] The normalized direction vector of the displacement change is

[0145] ;

[0146] in The change in displacement The initial position, The target location.

[0147] For each step , displacement increment Assign the normalized direction vectors to the three-dimensional coordinate axes to obtain... Starting from the initial position, the displacement increments at each step are accumulated to obtain the displacement interpolation sequence for each coordinate axis in three-dimensional space. ,in ,include axis, axis, The three coordinate components of the axis , , .

[0148] S54. The interpolation increments in the attitude direction are distributed according to the normalized direction vector of the attitude change, and the interpolation sequences of the three attitude angles are accumulated.

[0149] The normalized direction vector of the attitude change is

[0150] ;

[0151] in For attitude change, The initial attitude angle, The target attitude angle.

[0152] For each step Incremental attitude change Assign the normalized direction vector to the three attitude angles to obtain... Starting from the initial pose, the increments of pose change at each step are accumulated to obtain the interpolation sequence of the three pose angles. ,in It includes three attitude angle components: roll angle, pitch angle, and yaw angle.

[0153] S55. Combine the displacement interpolation sequence and the attitude interpolation sequence to output a synchronized displacement-attitude interpolation sequence.

[0154] The output result is ,in This ensures that translation and attitude are strictly synchronized in each discrete step. The interpolation sequence ensures that displacement and attitude remain deeply coupled in motion, that is, the attitude remains relatively stable when the displacement accelerates or decelerates in the acceleration segment, and the attitude is mainly adjusted when the displacement moves smoothly in the uniform segment, so as to achieve continuous, smooth and safe autonomous scanning of the probe.

[0155] By combining multi-constraint optimization and synchronous interpolation sequence generation, the method of this invention achieves deep coupling between displacement motion state and posture adjustment requirements while ensuring strict displacement hit and accurate posture. This significantly improves the motion smoothness, fit stability and image acquisition quality of the ultrasonic robot probe during autonomous scanning. Figure 3 A comparative schematic diagram of the traditional split optimization method and the method of this invention is shown, wherein, Figure 3 The vertical axis represents normalized velocity. Displacement velocity and attitude angular velocity are normalized relative to their respective maximum values ​​to visually compare their temporal synchronization and phased change characteristics. As shown in the figure, in traditional methods, displacement and attitude are optimized independently for the length of the velocity transition segment, leading to k... s ≠k θ The two movements are not synchronized, and the rapid change in attitude during the acceleration phase may cause fluctuations in contact force, affecting the probe's contact stability; however, the method of this invention shares the variable speed phase length parameter k. This allows for completely synchronized switching of displacement and posture during motion phases. In the variable speed phase, the posture is suppressed and constrained to maintain relative stability. In the uniform speed phase, posture compensation and adjustment are performed when the displacement is stable, effectively avoiding motion incoordination problems and significantly improving contact force stability and ultrasound image quality.

[0156] Furthermore, this invention provides a motion interpolation system for autonomous scanning by an ultrasonic robot, used to implement the motion interpolation method described above. The system includes a memory and a processor, which are connected via a bus. The memory can be a read-only memory, random access memory, flash memory, or other non-volatile storage medium used to store computer programs and data. The processor can be a central processing unit, digital signal processor, microcontroller, or field-programmable gate array, or other device with information processing capabilities. The memory stores a computer program implementing the above-described motion interpolation method. When the processor executes the computer program, it follows the process described in the above method embodiment to obtain the input parameters required for interpolation, construct a two-phase piecewise model, establish displacement closure constraints and attitude closure constraints, jointly solve multiple constraint conditions to optimize the variable speed segment length parameter, calculate the interpolation increment, and output a synchronized displacement-attitude interpolation sequence, thereby achieving adaptive motion interpolation control of the ultrasonic robot probe during autonomous scanning.

[0157] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A motion interpolation method for ultrasound robotic autonomous scanning, characterized in that, The method comprises the following steps: S1, obtaining input parameters required for interpolation, calculating displacement change and attitude change according to the input parameters; S2, determining a motion mode according to the displacement change and the attitude change, constructing a two-phase segmented model according to the determined motion mode, the two-phase segmented model comprising a variable-speed segment and a constant-speed segment, so that the displacement and the attitude are executed synchronously in time sequence, and the attitude angular acceleration is subjected to a restraining constraint in the variable-speed segment, and the attitude is adjusted by compensation in the constant-speed segment; S3, establishing a displacement closed constraint condition and an attitude closed constraint condition based on the two-phase segmented model, wherein the displacement closed constraint condition ensures that the displacement hits by compensation at the end of the displacement, and the attitude closed constraint condition is used to calculate the attitude compensation acceleration in the constant-speed segment, so as to ensure that the attitude is in place by the attitude compensation in the constant-speed segment; S4, within the value range of the variable-speed segment length parameter, jointly solving the displacement closed constraint condition, the attitude closed constraint condition, the attitude angular acceleration restraining constraint and the speed non-negative constraint, and selecting the maximum variable-speed segment length satisfying all the constraint conditions as an optimization result; S5, according to the optimization result, respectively calculating the interpolation increments of the displacement and the attitude in the variable-speed segment and the constant-speed segment and accumulating them, and outputting the synchronized displacement-attitude interpolation sequence; Step S3 specifically comprises: S31, calculating the cumulative displacement of the displacement direction in the variable-speed segment and the cumulative attitude change of the attitude direction in the variable-speed segment according to the two-phase segmented model; S32, obtaining the remaining displacement in the constant-speed segment according to the difference between the modulus of the displacement change and the cumulative displacement in the variable-speed segment, and obtaining the remaining attitude change in the constant-speed segment according to the difference between the modulus of the attitude change and the cumulative attitude change in the variable-speed segment; S33, calculating the theoretical step number of the constant-speed segment based on the remaining displacement in the constant-speed segment and the average speed of the displacement direction, taking the integer part of the theoretical step number of the constant-speed segment as the actual step number of the constant-speed segment, and compensating the displacement corresponding to the decimal part of the theoretical step number of the constant-speed segment at the end of the displacement, to establish the displacement closed constraint condition; S34, establishing the attitude closed constraint condition, and solving the attitude compensation acceleration in the constant-speed segment according to the expression of the attitude closed constraint condition and the cumulative attitude change in the constant-speed segment; Step S5 specifically comprises: S51, calculating the interpolation increment of the displacement direction and the attitude direction at each step in the variable-speed segment according to the variable-speed segment length determined by the optimization result; S52, calculating the interpolation increment of the displacement direction and the attitude direction at each step in the constant-speed segment, wherein the attitude direction is adjusted by the attitude compensation acceleration in the constant-speed segment; S53, distributing the interpolation increment of the displacement direction according to the normalized direction vector of the displacement change, and accumulating to obtain the displacement interpolation sequence of each coordinate axis in the three-dimensional space; S54, distributing the interpolation increment of the attitude direction according to the normalized direction vector of the attitude change, and accumulating to obtain the interpolation sequence of the three attitude angles; S55, combining the displacement interpolation sequence and the attitude interpolation sequence, and outputting the synchronized displacement-attitude interpolation sequence.

2. A motion interpolation method for ultrasound robotic autonomous scanning as claimed in claim 1, wherein, In step S1, the input parameters comprise an end effector initial position, a target position, a displacement direction initial speed, an initial attitude angle, a target attitude angle, an attitude direction initial angular velocity, an attitude angular acceleration maximum allowed value and a total interpolation step number.

3. A motion interpolation method for ultrasound robotic autonomous scanning as defined in claim 1, wherein, Step S2 specifically comprises: S21, calculate the average speed of displacement direction according to the displacement variation, and calculate the average angular velocity of attitude direction according to the attitude variation; S22, determine the motion mode of displacement according to the relationship between the average speed of displacement direction and the initial speed, and determine the motion mode of attitude according to the relationship between the average angular velocity of attitude direction and the initial angular velocity, the motion mode including the mode of accelerating first and then uniform speed, and the mode of decelerating first and then uniform speed; S23, construct the basic framework of the two-phase segmented model of the length parameter of the variable speed section shared by displacement and attitude according to the determined motion mode, the displacement and the attitude are executed synchronously in time sequence, the basic framework sets the speed to change linearly in the variable speed section, and sets the speed to keep constant in the uniform speed section; S24, apply angular acceleration suppression constraint to the attitude direction in the variable speed section of the two-phase segmented basic framework, the suppression constraint is achieved by limiting the attitude angular acceleration not to exceed the product of the decay coefficient and the maximum allowed value of the attitude angular acceleration; S25, set compensation acceleration to the attitude direction in the uniform speed section of the two-phase segmented basic framework, the compensation acceleration is used to complete the attitude adjustment, and the absolute value of the compensation acceleration does not exceed the maximum allowed value of the attitude angular acceleration, thereby forming a complete two-phase segmented model.

4. The motion interpolation method for autonomous scanning of an ultrasonic robot according to claim 3, wherein, for the displacement direction, the two-phase segmented model expression of the speed is: ； for the attitude direction, the two-phase segmented model expression of the angular velocity is: ； wherein, is the velocity of the variable speed segment, is the angular velocity of the variable speed segment, is the current step number, N is the total number of interpolation steps; is the variable speed segment length parameter, indicating the number of steps that the variable speed segment lasts, when is the variable speed segment, when is the constant speed segment; is the initial velocity of the displacement direction, is the displacement acceleration, is the average velocity of the displacement direction, is the initial angular velocity of the attitude direction, is the angular acceleration of the variable speed segment of the attitude, satisfying the constraint condition , is the attenuation coefficient, is the maximum allowed value of the angular acceleration of the attitude; is the attitude compensation acceleration of the constant speed segment, satisfying the constraint condition .

5. The motion interpolation method for autonomous scanning of an ultrasonic robot according to claim 1, wherein, the expression of the displacement closure constraint condition is: ； wherein, is the cumulative displacement amount of the variable speed section; is the average speed of the displacement direction; is the actual step count of the constant speed section, and is the integer part of the theoretical step count of the constant speed section; is the fractional part of the theoretical step count of the constant speed section, ; is the length of the displacement change amount; is the displacement amount compensated at the end of the displacement; the expression of the attitude closure constraint condition is: ； wherein, is the accumulated attitude change amount for the variable speed section; is the accumulated attitude change amount for the constant speed section; is the modulus of the attitude change amount; the expression of the attitude compensation acceleration in the uniform speed section is: ； wherein, is the constant acceleration of the uniform segment, satisfying the constraint ; is the length of the uniform segment, ; is the initial angular velocity of the uniform segment, ; is the initial angular velocity; is the angular acceleration of the variable segment; is the length parameter of the variable segment; is the total number of interpolation steps; is the maximum allowed value of the angular acceleration.

6. A motion interpolation method for ultrasound robotic autonomous scanning as defined in claim 1, wherein, Step S4 specifically includes: The value range of the variable speed stage length parameter is determined as wherein is determined by the minimum acceleration / deceleration ratio, is the total interpolation step number; iterative optimization is performed on the candidate values in the value range, for each candidate value, the displacement acceleration is calculated according to the displacement closure constraint condition, and it is checked whether the cumulative displacement amount of the variable speed section exceeds the modulus of the displacement variation; the attitude compensation acceleration in the uniform speed section is calculated according to the attitude closure constraint condition and the variable speed section attitude angular acceleration suppression constraint, and it is checked whether the absolute value thereof exceeds the maximum allowed value of the attitude angular acceleration, and the speed non-negative constraint is checked, and the maximum variable speed section length is selected from the candidate values satisfying all the constraint conditions as the optimization result.

7. The motion interpolation method for autonomous scanning of an ultrasonic robot according to claim 6, wherein, the expression of the displacement acceleration is: ； wherein, is the displacement acceleration; is the length of the displacement change; is the initial velocity; the expression of the cumulative attitude variation of the variable speed section is: ； wherein, is the maximum attitude change amount that can be completed by the variable speed stage; is the initial angular velocity; is the decay coefficient; is the maximum allowed value of the attitude angular acceleration.

8. A motion interpolation system for ultrasound robotic autonomous scanning, characterized in that, a memory and a processor, the memory stores a computer program, and the processor implements the motion interpolation method according to any one of claims 1-7 when executing the computer program.

Images