Ultrasound scanning robot motion planning method, device, equipment and storage medium

Abstract

The application relates to an ultrasonic scanning robot motion planning method, device, equipment and storage medium, the method comprising the following steps: determining whether there is a sharp turn condition according to position information of adjacent moments of an ultrasonic probe; determining whether there is a limiting condition on a running path through the ultrasonic probe; if there is a sharp turn condition and / or a limiting condition, calculating an interpolation step length according to current position information and target position information of the ultrasonic probe; and gradually adjusting the motion state of the ultrasonic probe according to the interpolation step length. The ultrasonic scanning robot motion planning method, device, equipment and storage medium provided in the embodiment of the application, wherein the method determines whether there is a sharp turn and / or a limiting condition of the ultrasonic probe, and if there is, the distance between the current position and the final stop and / or turning position is calculated, the interpolation step length of each step is sequentially calculated, and the motion reliability of the ultrasonic scanning robot is ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of ultrasonic scanning technology, and in particular to an ultrasonic scanning robot motion planning method, device, equipment and storage medium. BACKGROUND

[0002] The existing ultrasonic scanning robot, especially the ultrasonic scanning robot with a split design, uses a profile probe used by a doctor to communicate and control the movement of an ultrasonic probe used by a patient, thereby realizing scanning of a lesion area.

[0003] However, the ultrasonic probe used by the patient will appear sharp turns and limit conditions at the lesion, especially the edge area, in the actual scanning process, because the mechanical arm is linked with the ultrasonic probe for scanning. The sharp stop method will cause damage to the stability of the mechanical arm and the patient's body, so a reliable motion planning method for the ultrasonic scanning robot is needed. SUMMARY

[0004] The present application provides an ultrasonic scanning robot motion planning method, device, equipment and storage medium, aiming to ensure the reliability of the ultrasonic probe during motion.

[0005] In a first aspect, the present application provides an ultrasonic scanning robot motion planning method, comprising:

[0006] According to the position information of the ultrasonic probe at adjacent time points, it is determined whether there is a sharp turn condition;

[0007] Whether there is a limit condition on the travel path is obtained by the ultrasonic probe;

[0008] If there is a sharp turn condition and / or a limit condition, the interpolation step is calculated according to the current position information and the target position information of the ultrasonic probe;

[0009] According to the interpolation step, the motion state of the ultrasonic probe is gradually adjusted.

[0010] Optionally, the position information of the ultrasonic probe at adjacent time points includes the current position information, the previous time position information and the next time position information under the same time interval.

[0011] The sharp turn condition includes a displacement sharp turn condition and / or a posture sharp turn condition.

[0012] Optionally, according to the position information of the ultrasonic probe at adjacent time points, it is determined whether there is a sharp turn condition, specifically comprising:

[0013] The first difference between the current position information and the previous time position information and the second difference between the next time position information and the current position information are obtained respectively;

[0014] calculating a vector angle of the first difference value and the second difference value;

[0015] If the vector angle is greater than an angle threshold, a sharp turn exists.

[0016] Optionally, the interpolation step is calculated, specifically including:

[0017] calculating a distance value of the current position and the target position;

[0018] If the distance value is less than a plus-braking distance threshold of the ultrasound probe, the interpolation step is calculated;

[0019] If the distance value is greater than the plus-braking distance threshold of the ultrasound probe, a motion time is determined by comparing a plus-braking time period and an acceleration motion time of the ultrasound probe, and the interpolation step is calculated; wherein the acceleration motion time is a motion time period containing acceleration motion.

[0020] Optionally, if the plus-braking time period is greater than the acceleration motion time, the acceleration motion time is selected as the motion time.

[0021] If the plus-braking time period is less than the acceleration motion time, the plus-braking time period is selected as the motion time.

[0022] Optionally, after the interpolation step is calculated, the interpolation step is further adjusted according to the motion period.

[0023] Optionally, the limiting condition includes collision, angle limitation, and position limitation.

[0024] In a second aspect, an embodiment of the present application provides an ultrasound scanning robot motion planning device, which applies the ultrasound scanning robot motion planning method provided in the first aspect, and includes:

[0025] A sharp turn determination module is configured to determine whether a sharp turn condition exists according to position information of the ultrasound probe at adjacent time points.

[0026] A limiting determination module is configured to determine whether a limiting condition exists in the travel path by the ultrasound probe.

[0027] An interpolation step calculation module is configured to calculate an interpolation step according to current position information and target position information of the ultrasound probe when the sharp turn condition and / or the limiting condition exist.

[0028] A motion state adjustment module is configured to gradually adjust a motion state of the ultrasound probe according to the interpolation step.

[0029] In a third aspect, an embodiment of the present application provides an electronic device, which includes one or more processors.

[0030] A memory is configured to store one or more programs.

[0031] When one or more programs are executed by one or more processors, the one or more processors implement the ultrasonic scanning robot motion planning method provided by any embodiment of the present application.

[0032] In a fourth aspect, an embodiment of the present application provides a storage medium containing computer executable instructions for performing the ultrasonic scanning robot motion planning method provided by any embodiment of the present application when executed by a computer processor.

[0033] The ultrasonic scanning robot motion planning method, device, equipment and storage medium provided by the embodiment of the present application, wherein the method calculates the distance between the current position and the final stop and / or turning position if there is a sharp turn and / or limit position, and sequentially calculates the interpolation step of each step, thereby ensuring the reliability of the ultrasonic scanning robot motion. BRIEF DESCRIPTION OF DRAWINGS

[0034] Figure 1 A flow chart of an ultrasonic scanning robot motion planning method provided by an embodiment of the present application;

[0035] Figure 2 A flow chart of determining a sharp turn condition in an ultrasonic scanning robot motion planning method provided by an embodiment of the present application;

[0036] Figure 3 A flow chart of calculating an interpolation step in an ultrasonic scanning robot motion planning method provided by an embodiment of the present application;

[0037] Figure 4 A flow chart of determining a plus-braking time period and an acceleration motion time in an ultrasonic scanning robot motion planning method provided by an embodiment of the present application;

[0038] Figure 5 A structural schematic diagram of an ultrasonic scanning robot motion planning device provided by an embodiment of the present application;

[0039] Figure 6 A structural schematic diagram of an ultrasonic scanning robot motion planning equipment provided by an embodiment of the present application;

[0040] Figure 7 A schematic diagram of a displacement sharp turn in an ultrasonic scanning robot motion planning method provided by an embodiment of the present application;

[0041] Figure 8 A schematic diagram of a posture sharp turn in an ultrasonic scanning robot motion planning method provided by an embodiment of the present application. DETAILED DESCRIPTION

[0042] The application will be described in further detail below with reference to the drawings and embodiments. It is to be understood that the specific embodiments described herein are merely illustrative of the application and are not to be used to limit the scope of the application. In addition, it should be noted that, for the sake of brevity, only the portions of the drawings that are needed to understand the application have been shown in the drawings.

[0043] The ultrasonic scanning robot used at present adopts a linkage mode of a mechanical arm and an ultrasonic probe to perform scanning, so that sharp turns and position limits occur at a lesion, especially at an edge region. The sharp stop mode will cause damage to the stability of the mechanical arm and the body of a patient, and thus a reliable motion planning method for the ultrasonic scanning robot is needed.

[0044] Embodiment one

[0045] The application aims at the above-mentioned deficiencies, and proposes an ultrasonic scanning robot motion planning method, as shown in the accompanying drawings, which comprises the following steps. Figure 1

[0046] S10: Determine whether there is a sharp turn condition according to position information of the ultrasonic probe at adjacent time points. A transition point needs to be added to the moving position of the ultrasonic probe in the determination process of the sharp turn, and the transition point is a point at which the ultrasonic probe is decelerated to a speed of 0, and the moving direction is changed at the transition point.

[0047] Generally, two determination modes of displacement sharp turn condition and / or posture sharp turn are used in the determination process of the sharp turn. The difference between the two modes is that the posture sharp turn is based on a tool coordinate system of the ultrasonic scanning robot, and a rotation matrix is used to transfer a vector of the ultrasonic probe within adjacent time points to the tool coordinate system, and the determination is performed in the coordinate system.

[0048] In the case of determining the displacement sharp turn, as shown in the accompanying drawings, the step S10 specifically comprises the following steps. Figure 2

[0049] S11: Obtain a first difference value of the current position information and the previous time position information and a second difference value of the next time position information and the current position information, respectively.

[0050] Specifically, the previous time position information is P tar0 , the current position information is P cur , and the next time position information is P tar , and the first difference value I1 and the second difference value I2 are calculated, respectively.

[0051] I1=P cur -P tar0

[0052] I2=P tar -P cur ​​

[0053] S12: Calculate the vector angle between the first difference and the second difference;

[0054] The angle calculation formula is as follows:

[0055] S13: If the vector angle θ p is greater than the angle threshold ε p , there is a sharp turn. An optional implementation is that ε p is 120°, and the vector directions of the first difference I1 and the second difference I2 are as shown in the following figure. Figure 7

[0056] Similarly, in the process of determining the sharp turn of the posture, first arrange a point Q(0, 0, -d) in the Z-axis direction of the tool coordinate system of the ultrasonic scanning robot, and convert the position information at the previous moment P tar0 , the current position information P cur and the next moment position information P tar into position information in the tool coordinate system through the rotation matrix in the tool coordinate system, that is:

[0057] R tar0 = P tar0 · Q

[0058] R cur = P cur · Q

[0059] R tar = P tar · Q

[0060] Wherein, R tar0 , R cur , R tar represent the position information at the previous moment, the previous position information, and the next moment position information in the work coordinate system respectively.

[0061] Calculate the corresponding first difference R1 and the second difference R2 in the tool coordinate system:

[0062] R1 = R cur - R tar0

[0063] R2 = R tar - R cur

[0064] The corresponding angle calculation formula is as follows:

[0065]

[0066] When θ r is greater than the angle threshold ε r in the tool coordinate system, there is a sharp turn., it is determined that there is a posture sharp turn. The included angle threshold value ε in the tool coordinate system r is 120°, that is, the included angle between the moving direction of the ultrasonic probe at the current moment and the moving direction at the next moment, as shown in Figure 8 .

[0067] It should be noted here that in the case of determining the existence of displacement sharp turn and / or posture sharp turn, the transition point can be understood as the point obtained by continuing to slide a distance d cur at the current P m position.

[0068] S20: Obtain whether the travel path has a limit condition through the ultrasonic probe; the limit condition includes collision, angle limitation, and position limitation, etc. The determination method can be a camera or sensor arranged at the end of the ultrasonic probe.

[0069] S30: If there is a sharp turn condition and / or a limit condition, calculate the interpolation step length according to the current position information and the target position information of the ultrasonic probe; it should be noted here that the interpolation step length is usually used to gradually interpolate between the current position and the transition point, and the interpolation step length is adjusted for each step.

[0070] Specifically, as shown in Figure 3 , the process of calculating the interpolation step length includes:

[0071] S31: Calculate the distance value of the current position and the target position; the above target position can be regarded as the transition point, so the distance value is the distance from the previous position to the complete stop position. In general, the distance includes acceleration distance, constant speed distance, and deceleration distance (braking distance). And the above acceleration distance, constant speed distance, and deceleration distance (braking distance) have a total distance threshold, that is, the acceleration-braking distance threshold.

[0072] S32: If the distance value is less than the acceleration-braking distance threshold of the ultrasonic probe, the interpolation step length is calculated; assuming that the distance value is D T , and the acceleration-braking distance threshold is S acc-dec , when D T <S acc-dec , it indicates that the distance cannot include acceleration and constant speed processes, only deceleration process is adopted. The interpolation step length is: n = (1 + V dec ·t M )·n i

[0073] , where V dec represents the speed of the deceleration process, and t M represents the corresponding acceleration-braking time period. The obtained interpolation step length is the step length of one step, and then the subsequent interpolation step lengths are sequentially calculated.

[0074] S33: If the distance value is greater than the acceleration-deceleration distance threshold of the ultrasonic probe, compare the acceleration-deceleration time period t. M The acceleration time t of the ultrasound probe acc The motion time is determined, and the interpolation step size is calculated; where the acceleration time is the motion time period including the acceleration motion. It should be noted here that t... M It is a system preset value, representing the minimum time value for acceleration-uniform speed-deceleration.

[0075] Among them, such as Figure 4 As shown, S331: If the acceleration-braking time period is greater than the acceleration time t M >t acc Then the acceleration time is selected as the motion time;

[0076] n=(1+V dec ·t acc )·n i

[0077] S332: If the braking time period is less than the acceleration time t M <t acc Then select the acceleration-braking time period t M The duration of the movement, i.e.:

[0078] n=(1+V dec ·t M )·n i

[0079] Then by letting t M =t M +1, to calculate the interpolation step size for the next step.

[0080] S40: Adjust the motion state of the ultrasonic probe step by step according to the interpolation step size. Drive the robotic arm and the corresponding connected ultrasonic probe to move according to each interpolation step size calculated above.

[0081] It should also be noted that after calculating the interpolation step size, the process also includes adjusting the interpolation step size based on the motion cycle. This is because the influence of cycle fluctuations can cause the robotic arm to vibrate. Therefore, by correcting the ratio of the actual control cycle to the desired control cycle, the number of interpolation steps is adjusted, i.e.:

[0082] n n =(T cl / T dl )·n

[0083] Where T cl T represents the actual control period. dl This indicates the desired control period.

[0084] The ultrasonic scanning robot motion planning method provided by the embodiment of the present application can ensure the reliability of the ultrasonic scanning robot motion by judging whether the ultrasonic probe has a sharp turn and / or a limit condition, and sequentially calculating the interpolation step of each step by calculating the distance between the current position and the final stop and / or turning position if there is a sharp turn and / or a limit condition.

[0085] Embodiment two

[0086] The present application also provides an ultrasonic scanning robot motion planning device, as shown in the accompanying drawings, comprising: Figure 5

[0087] The sharp turn judgment module 01 is used to judge whether there is a sharp turn condition according to the position information of the ultrasonic probe at adjacent time points; wherein the sharp turn condition includes a displacement sharp turn condition and / or a posture sharp turn condition.

[0088] The sharp turn judgment module 01 is used to perform the following operations:

[0089] The first difference value of the current position information and the previous time position information and the second difference value of the next time position information and the current position information are obtained respectively;

[0090] The vector angle of the first difference value and the second difference value is calculated;

[0091] If the vector angle is greater than the angle threshold, there is a sharp turn.

[0092] The limit judgment module 02 is used to obtain whether there is a limit condition in the travel path through the ultrasonic probe; wherein the limit condition includes collision, angle limitation and position limitation.

[0093] The interpolation step calculation module 03 is used to calculate the interpolation step according to the current position information and the target position information of the ultrasonic probe when there is a sharp turn condition and / or a limit condition.

[0094] The interpolation step calculation module 03 is configured to perform the following operations:

[0095] The distance value of the current position and the target position is calculated;

[0096] If the distance value is less than the acceleration-braking distance threshold of the ultrasonic probe, the interpolation step is calculated;

[0097] If the distance value is greater than the acceleration-braking distance threshold of the ultrasonic probe, the acceleration motion time is compared with the acceleration motion time of the ultrasonic probe to determine the motion time, and the interpolation step is calculated; wherein the acceleration motion time is a motion time period containing acceleration motion.

[0098] ​If the acceleration-braking time period is greater than the acceleration time, then the acceleration time is selected as the motion time.

[0099] If the acceleration-braking time period is less than the acceleration time, then the acceleration-braking time period is selected as the motion time. Furthermore, after calculating the interpolation step size, the process also includes adjusting the interpolation step size based on the motion period.

[0100] The motion state adjustment module 04 is used to gradually adjust the motion state of the ultrasonic probe according to the interpolation step size.

[0101] The ultrasonic scanning robot motion planning device provided in this embodiment of the invention uses the same technical means as the ultrasonic scanning robot motion planning method to achieve the same technical effect, which will not be described in detail here.

[0102] Example 3

[0103] Figure 6 This is a schematic diagram of the structure of an ultrasonic scanning robot motion planning device provided in an embodiment of the present invention, as shown below. Figure 6 As shown, the motion planning device for the ultrasonic scanning robot includes a processor 610, a memory 620, an input device 630, and an output device 640; the number of processors 610 in the ultrasonic scanning robot motion planning device can be one or more. Figure 6 Taking a processor 610 as an example; the processor 610, memory 620, input device 630, and output device 640 in the ultrasonic scanning robot motion planning device can be connected via a bus or other means. Figure 6 Taking the example of a connection between China and Israel via a bus.

[0104] The memory 620, as a computer-readable storage medium, can be used to store software programs, computer-executable programs, and modules, such as the program instructions / modules corresponding to the ultrasonic scanning robot motion planning method in this embodiment of the invention (e.g., a sharp turn determination module, a limit determination module, a side interpolation step length calculation module, and a motion state adjustment module). The processor 610 executes various functional applications and data processing of the ultrasonic scanning robot motion planning device by running the software programs, instructions, and modules stored in the memory 620, thereby realizing the aforementioned ultrasonic scanning robot motion planning method.

[0105] The memory 620 can include a program storage area and a data storage area, where the program storage area can store an operating system, at least one application required by a function, and the data storage area can store data created according to the use of the terminal, etc. In addition, the memory 620 can include a high-speed random access memory, and can further include a non-volatile memory, such as at least one magnetic disk storage device, a flash memory device, or other non-volatile solid-state memory device. In some examples, the memory 620 can further include a memory disposed remotely with respect to the processor 610, which can be connected to the ultrasound scanning robot motion planning device through a network. Examples of the above network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0106] The input device 630 can be used to receive input digital or character information, and to generate key signal inputs related to user settings and function controls of the ultrasound scanning robot motion planning device. The output device 640 can include a display device such as a display screen.

[0107] Embodiment Four

[0108] The embodiment four of the present application also provides a storage medium containing computer executable instructions, which, when executed by a computer processor, are used to perform an ultrasound scanning robot motion planning method, comprising:

[0109] According to the position information of the ultrasound probe at adjacent time points, it is determined whether there is a sharp turn condition;

[0110] It is determined whether there is a limit condition on the travel path through the ultrasound probe;

[0111] If there is a sharp turn condition and / or a limit condition, an interpolation step is calculated according to the current position information and the target position information of the ultrasound probe;

[0112] According to the interpolation step, the motion state of the ultrasound probe is adjusted step by step.

[0113] Of course, the storage medium containing computer executable instructions provided by the embodiment of the present application is not limited to the above method operations, and can also perform related operations in the ultrasound scanning robot motion planning method provided by any embodiment of the present application.

[0114] Through the above description of the embodiments, those skilled in the art can clearly understand that the present application can be realized by means of software and necessary universal hardware, and of course can also be realized by hardware, but in many cases the former is a better embodiment. Based on such understanding, the technical solutions of the present application can be embodied in the form of a software product in essence or in the form of a part of the prior art that makes a contribution, and the computer software product can be stored in a computer readable storage medium, such as a floppy disk, a read-only memory (ROM), a random access memory (RAM), a FLASH, a hard disk, or an optical disc, etc., and includes a number of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute the methods of various embodiments of the present application.

[0115] It is worth noting that in the above embodiments of the ultrasonic scanning robot motion planning device, each unit and module included is only divided according to functional logic, but is not limited to the above division, as long as the corresponding functions can be realized; in addition, the specific names of each functional unit are only for easy mutual differentiation, and do not limit the protection scope of the present application.

[0116] Although the present application has been described in detail in the foregoing general description, specific embodiments and experiments, some modifications or improvements can be made on the basis of the present application, which is obvious to those skilled in the art. Therefore, these modifications or improvements made on the basis of not deviating from the spirit of the present application are within the scope of protection claimed by the present application.

Claims

1. An ultrasound scanning robot motion planning method, characterized by, The method comprises the following steps: determining whether there is a sharp turn condition according to position information of adjacent time of the ultrasonic probe; acquiring whether there is a limiting condition on the travel path through the ultrasonic probe; if there is a sharp turn condition and / or a limiting condition, calculating an interpolation step length according to current position information and target position information of the ultrasonic probe; adjusting the motion state of the ultrasonic probe step by step according to the interpolation step length; wherein the interpolation step length is used to gradually interpolate between the current position and a transition point, and the transition point is a point where the ultrasonic probe is decelerated to a speed of 0.

2. The ultrasound scanning robot motion planning method of claim 1, wherein, The position information of adjacent time of the ultrasonic probe includes current position information, previous time position information and next time position information under the same time interval; The sharp turn condition includes displacement sharp turn condition and / or attitude sharp turn condition.

3. The ultrasound scanning robot motion planning method of claim 2, wherein, The determination of whether there is a sharp turn condition according to the position information of adjacent time of the ultrasonic probe specifically comprises: respectively acquiring a first difference value of the current position information and the previous time position information and a second difference value of the next time position information and the current position information; calculating the vector angle of the first difference value and the second difference value; if the vector angle is greater than an angle threshold, there is a sharp turn.

4. The ultrasonic scanning robot motion planning method of claim 1, wherein, The calculation of the interpolation step length specifically comprises: calculating the distance value between the current position and the target position; if the distance value is less than the acceleration-braking distance threshold of the ultrasonic probe, the interpolation step length is calculated according to the distance value; if the distance value is greater than the acceleration-braking distance threshold of the ultrasonic probe, the acceleration motion time of the ultrasonic probe is compared with the acceleration-braking time period to determine the motion time, and the interpolation step length is calculated; wherein the acceleration motion time is a motion time period containing acceleration motion; the acceleration-braking distance threshold is the overall distance threshold of the acceleration distance, constant speed distance and deceleration distance of the ultrasonic probe from the current position to the complete stop position.

5. The ultrasonic scanning robot motion planning method of claim 4, wherein, if the acceleration-braking time period is greater than the acceleration motion time, the acceleration motion time is selected as the motion time; if the acceleration-braking time period is less than the acceleration motion time, the acceleration-braking time period is selected as the motion time.

6. The ultrasonic scanning robot motion planning method of claim 1, wherein, After the calculation of the interpolation step length, the interpolation step length is further adjusted according to the motion period.

7. The ultrasonic scanning robot motion planning method of claim 1, wherein, The limiting condition includes collision, angle limitation and position limitation.

8. An ultrasound scanning robot motion planning apparatus characterized by, The method comprises the following steps: a sharp turn determination module for determining whether there is a sharp turn condition according to position information of adjacent time of the ultrasonic probe; a limiting determination module for acquiring whether there is a limiting condition on the travel path through the ultrasonic probe; an interpolation step length calculation module for calculating an interpolation step length according to current position information and target position information of the ultrasonic probe if there is a sharp turn condition and / or a limiting condition; a motion state adjustment module for adjusting the motion state of the ultrasonic probe step by step according to the interpolation step length; wherein the interpolation step length is used to gradually interpolate between the current position and a transition point, and the transition point is a point where the ultrasonic probe is decelerated to a speed of 0.

9. An electronic device, comprising: The electronic device comprises: one or more processors; a memory to store one or more programs; when the one or more programs are executed by the one or more processors, cause the one or more processors to implement the method of robotic motion planning for ultrasound scanning as claimed in any one of claims 1-7.

10. A storage medium containing computer-executable instructions, wherein: the computer executable instructions, when executed by a computer processor, perform the method of robotic motion planning for ultrasound scanning as claimed in any one of claims 1-7.

Images