A robotic arm acoustic visual servo method, system and device for turbid water bodies

Abstract

The present invention discloses a method, system and device for acoustic and visual servoing of a robotic arm for turbid water bodies. Specifically, it includes: first setting the probe parameters and obtaining the imaging range; performing three-dimensional ultrasonic imaging detection and noise elimination on the target to obtain the three-dimensional ultrasonic imaging result; then performing target detection on the three-dimensional ultrasonic imaging result, obtaining the spatial coordinates and posture information of the target in the 2D ultrasonic probe coordinate system and adjusting it to the robotic arm coordinate system; calculating the Euler angles of the end posture of the robotic arm and the rotation angle of each joint, and calculating the angle deviation for linear fitting, converting it into a control signal and transmitting it to the robotic arm controller to drive the robotic arm to complete the target tracking task. The present invention uses a 2D ultrasonic probe to perform short-range real-time three-dimensional ultrasonic imaging in turbid water bodies and detect the target position, converting the acoustic vision into a robotic arm control signal, avoiding the problem that optical vision is difficult to clearly present the target in turbid water bodies, and realizing the servo control of the robotic arm under turbid water bodies.

Description

Technical Field

[0001] The invention belongs to the field of underwater target detection and tracking, and in particular relates to a mechanical arm acoustic visual servo method, system and device for turbid water bodies. Background Art

[0002] The total area of ​​the ocean accounts for 71% of the earth's total surface area. It is the second largest strategic space after land, sea, air and space. It is also the strategic development base for renewable energy, biological resources and metal deposits with the greatest development potential at present and in the future. The exploration and development of marine mineral resources, the construction and maintenance of underwater oil and gas platforms, etc. are inseparable from underwater robotic arms. At present, underwater robotic arms are all based on vision for servo control. However, the attenuation effect of water on light is more serious than that of air. In addition, the real underwater environment is extremely complex. For example, most of the underwater operating environment in the East China Sea is turbid water. The influence of turbid water on visual imaging is mainly the following: (1) The light beam disperses the energy of the imaging beam through the turbid water; (2) The scattering of light by small particles in the turbid water causes a lot of noise in the imaging results, reducing the signal-to-noise ratio of the image; (3) Under turbid water, the environment is dark and the light is weak.

[0003] These factors lead to extremely poor visibility of underwater images, with problems such as blurring, degradation, color cast, fogging, and even the problem of "unclear" images. In turbid water, vision-based robotic arm servo control has already shown problems in visual imaging, and a series of subsequent robotic arm servo control tracking target underwater operations are bound to have errors or mistakes.

[0004] There are currently two main solutions to the above-mentioned turbid underwater image problem: one is to design a turbid water image enhancement algorithm, and the other is to design a turbid water imaging device. Of the two solutions, the former increases the complexity of the processing algorithm and the computing power requirements, while the latter increases the complexity of the underwater operation manipulator system device and has higher requirements for hardware equipment.

[0005] Sonar technology is used to image submarine targets at ultra-long distances and within ultra-large ranges, and cannot perform high-resolution imaging of small submarine targets. In addition, due to the large imaging field of view of sonar technology, its imaging efficiency is slow, and it cannot perform ultra-fast frame rate imaging, which cannot meet the servo characteristics of the robotic arm. Summary of the invention

[0006] In view of the above-mentioned deficiencies in the prior art, the present invention provides a robotic arm acoustic visual servoing method, system and device for turbid water bodies.

[0007] In a first aspect, the object of the present invention is to achieve the following technical solution: a robotic arm acoustic visual servoing method for turbid water bodies, comprising:

[0008] S1. Adjust the 2D ultrasonic probe parameters according to the characteristics of turbid water; and set the imaging space depth according to the probe parameters to obtain the imaging range;

[0009] S2, performing three-dimensional ultrasonic imaging detection on the target based on the ultrasonic probe parameters and imaging range; using the half-diffusion angle of the array element acoustic beam to eliminate ultrasonic image noise and obtain a three-dimensional ultrasonic imaging result;

[0010] S3, performing target detection on the three-dimensional ultrasound imaging results to obtain the spatial coordinates and position information of the target in the 2D ultrasound probe coordinate system;

[0011] S4, adjusting the spatial coordinates and posture information of the target in the 2D ultrasonic probe coordinate system to the robotic arm coordinate system; calculating the Euler angle of the robotic arm end posture and the rotation angle of each joint of the robotic arm through the spatial coordinates and posture information;

[0012] S5. The rotation angle deviation is obtained through the rotation angle of each joint and the initial angle of the robot arm, and a linear fitting is performed. The result is converted into a control signal and transmitted to the robot arm controller to drive the robot arm to complete the target tracking task.

[0013] Furthermore, the adjustment of the 2D ultrasonic probe parameters according to the turbid water characteristics is specifically as follows: according to the longitudinal wave sound velocity v and attenuation characteristics of the turbid water, the frequency f and the number N of the single probe in the probe are determined. 2 , chip size d1 and single probe size d2.

[0014] Furthermore, the 2D ultrasound probe is a planar array probe, and N 2 The single probes are combined into a square array probe suitable for ultrasonic imaging of turbid water bodies, and the center distance between the square array probes is the size d2 of the single probes.

[0015] Furthermore, the imaging space depth is set according to the probe parameters to obtain the imaging range, which specifically includes:

[0016] Through the center array element position of the 2D ultrasound probe, the perpendicular line of the probe plane is taken as the z-axis along the ultrasonic emission direction, the intersection of the z-axis and the center array element plane is taken as the origin, and the two directions extending along the array arrangement are taken as the x-axis and y-axis respectively to define the imaging coordinate system; the upper and lower boundary coordinate values ​​of the z-axis of the 2D ultrasound probe imaging coordinate system are set as z min and z max ;

[0017] According to the probe parameters, the directional characteristics of the array in turbid water are calculated, and the half diffusion angle θ of the array in turbid water is calculated using the directional characteristics. -6dB , based on the upper and lower boundary coordinates of the z-axis, using the half diffusion angle θ -6dB Calculate the boundary value x of the array probe in the x-axis and y-axis directions in the xy plane min 、xmax ,y min and max , as the effective area of ​​the sound beam, determine the imaging area range of the 2D ultrasonic probe in the turbid water body;

[0018] The directional characteristics of the 2D ultrasonic probe array along the x-axis and y-axis in turbid water are:

[0019] The half diffusion angle of the 2D ultrasonic probe array in turbid water is: θ -6dB , where θ -6dB satisfy Where v represents the longitudinal wave speed of turbid water;

[0020] The x-axis boundary value of the 2D ultrasound probe in the xy plane is:

[0021] The y-axis boundary value of the 2D ultrasound probe in the xy plane is:

[0022] Furthermore, the three-dimensional ultrasound imaging in S2 includes three-dimensional ultrasound full-focus imaging, specifically: performing N 2 Transmit times, receive N times each time 2 The echo data of array elements is obtained 2 ×N 2 Groups of data, each of which is an A-scan signal;

[0023] The imaging area is divided into pixels, and the intervals of the pixels in the x, y, and z directions are dx, dy, and dz respectively;

[0024] The transit time of each transmitting-receiving array element pair at each imaging pixel is calculated, and the imaging algorithm is optimized using the semi-diffusion angle of the array element acoustic beam. If a certain pixel is within the semi-diffusion space of a certain transmitting-receiving array element pair, the echo data is delayed and superimposed by the transit time. Otherwise, the contribution of the transmitting-receiving array element pair to the imaging amplitude of the pixel is discarded to avoid the introduction of noise signals. Ultrafast frame rate three-dimensional ultrasonic full-focusing imaging of turbid underwater targets at 20 Hz or above is performed.

[0025] Furthermore, S3 specifically includes: finding the maximum amplitude pixel point in the three-dimensional ultrasound imaging result, and inferring the index [L, M, N] of the maximum amplitude pixel point in the three-dimensional matrix, thereby calculating the three-dimensional spatial coordinates of the target in the array probe coordinate system, and the calculation methods are respectively x=x min +Ldx,y=y min +Mdy,z=z min +Ndz; where x min ,ymin and z min They are respectively the lower boundary coordinate values ​​of the x, y, and z axes of the 2D ultrasound probe imaging coordinate system.

[0026] Furthermore, the S4 specifically includes: according to the posture relationship matrix of the 2D ultrasonic probe in the robot arm base coordinate system The three-dimensional space coordinates (x, y, z) of the target in the array coordinate system are converted into the three-dimensional space coordinates (x w ,y w ,z w ), the calculation relationship is: And set the detection target posture and the robot end effector posture information to be the same as Get the position information T of the target object in the robot base coordinate system:

[0027]

[0028] According to the pose information T in the robot base coordinate system, the Euler angle Q of the robot end pose is calculated:

[0029]

[0030] The posture information T is converted into the rotation angle θ of each joint of the six-degree-of-freedom manipulator through the robot inverse kinematics; according to the following formula:

[0031]

[0032]

[0033] Obtain the rotation angle θ of each joint of the six-degree-of-freedom robotic arm:

[0034]

[0035] Where r and p represent the rotation operator and translation operator respectively.

[0036] Furthermore, the S5 specifically includes: according to the initial joint angle θ' of the robot arm, calculating the rotation angle deviation Δθ required for each joint of the robot arm to reach the desired characteristic point, performing linear fitting on the rotation angle deviation, converting it into a control signal and transmitting it to the robot arm controller to drive the robot arm to complete the target tracking task. The specific calculation formula is as follows:

[0037]

[0038] The linear fitting function is:

[0039] Δθ=ωt,

[0040] Where t is time and ω is angular velocity.

[0041] In a second aspect, the present invention provides a robotic arm acoustic visual servo system for turbid water bodies, comprising:

[0042] An imaging range control module is used to adjust the 2D ultrasonic probe parameters according to the characteristics of turbid water; and to set the imaging space depth according to the probe parameters to obtain the imaging range;

[0043] The three-dimensional ultrasonic imaging detection module is used to perform three-dimensional ultrasonic imaging detection on the target based on the ultrasonic probe parameters and imaging range; the ultrasonic image noise is eliminated by using the half-diffusion angle of the array element sound beam to obtain the three-dimensional ultrasonic imaging result;

[0044] The target detection module is used to perform target detection on the 3D ultrasound imaging results and obtain the spatial coordinates and position information of the target in the 2D ultrasound probe coordinate system;

[0045] The acoustic-visual conversion module is used to adjust the spatial coordinates and posture information of the target in the 2D ultrasound probe coordinate system to the robotic arm coordinate system; calculate the Euler angle of the robotic arm end posture and the rotation angle of each joint of the robotic arm through the spatial coordinates and posture information;

[0046] The robot arm servo module is used to obtain the angle deviation through the rotation angle of each joint and the initial angle of the robot arm, and perform linear fitting, which is converted into a control signal and transmitted to the robot arm controller to drive the robot arm to complete the target tracking task.

[0047] In a third aspect, the present invention specification provides a robotic arm acoustic visual servoing device for turbid water bodies, comprising a memory and one or more processors, wherein the memory stores executable code, and when the processor executes the executable code, it is used to implement a robotic arm acoustic visual servoing method for turbid water bodies.

[0048] Beneficial effects of the present invention:

[0049] The present invention utilizes 2D ultrasonic probes (including but not limited to planar array probes and circular array probes) to perform short-range (<2m) real-time three-dimensional ultrasonic imaging (including but not limited to fan scanning, line scanning, synthetic aperture three-dimensional ultrasonic imaging, three-dimensional ultrasonic full-focus imaging, three-dimensional ultrasonic plane wave imaging, etc.) in turbid water bodies. The acoustic vision method of ultrasonic imaging effectively avoids the problems of blurring, degradation, color cast, and atomization of optical vision in turbid water bodies, eliminates the influence of turbid water bodies on optical visual imaging, and does not require the use of turbid water image enhancement algorithms with higher computing power. At the same time, it is also unnecessary to connect a turbid water imaging device with higher hardware complexity at the end of the robotic arm to perform high-quality imaging of targets in turbid water bodies, thereby solving the problem of poor imaging quality and inability to image targets in turbid water bodies with existing cameras.

[0050] The present invention realizes acoustic vision technology different from sonar technology by designing a 2D ultrasonic probe for turbid water bodies and using a three-dimensional ultrasonic method with an ultra-fast frame rate. Compared with sonar technology, acoustic vision technology performs short-range (<2m) high-resolution and refined imaging of small targets in the water, while sonar technology is aimed at imaging seabed targets at ultra-long distances and ultra-large ranges, and cannot perform high-resolution imaging of small seabed targets. In addition, due to the large imaging field of view of sonar technology, its imaging efficiency is slow, and ultra-fast frame rate imaging cannot be performed, which cannot meet the servo characteristics of the robotic arm, while the proposed acoustic vision technology can perform ultra-fast frame rate imaging and meet the servo characteristics of the robotic arm.

[0051] The present invention replaces optical camera imaging with ultrasonic imaging, and uses the ultrasonic imaging results as input to perform servo control on the robot arm in turbid water bodies, thereby solving the problems of poor visual servo effect or visual servo error in traditional turbid water bodies. The present invention can be widely used in submarine operation scenarios, such as submarine pipeline weld inspection, and has broad market prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A flow chart of a robotic arm acoustic visual servoing method for turbid water bodies provided by an exemplary embodiment;

[0053] Figure 2 An exploded diagram of an ultrasonic surface array probe for turbid water imaging provided by an exemplary embodiment;

[0054] Figure 3 A schematic diagram of a method for calculating the imaging area range of an ultrasonic surface array probe for turbid water imaging provided by an exemplary embodiment;

[0055] Figure 4 A diagram showing the result of three-dimensional ultrasonic full-focus imaging of a turbid underwater target provided by an exemplary embodiment;

[0056] Figure 5 A schematic diagram of a robotic arm acoustic visual servoing device for turbid water bodies provided as an exemplary embodiment. DETAILED DESCRIPTION

[0057] The specific implementation modes of the present invention are further described in detail below with reference to the accompanying drawings.

[0058] It should be clear that the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0059] like Figure 1 As shown, a robotic arm acoustic visual servo method for turbid water bodies is provided, comprising the following steps:

[0060] Step S1, determining the parameters of a single probe in a combined 2D ultrasonic probe according to the characteristics of turbid water, and assembling a special 2D ultrasonic probe for turbid water underwater imaging;

[0061] like Figure 2 As shown in FIG. 3 , in one embodiment, the specific steps of step S1 are as follows:

[0062] Step S11: In this embodiment, the 2D ultrasonic probe is taken as an example of a planar array probe, and the single probe frequency f and the number N of the combined planar array probe are determined according to the longitudinal wave sound velocity v and attenuation characteristics of turbid water. 2 , chip size d1 and single probe size d2;

[0063] Step S12: Arrange N 2 The single probes are combined into a square array probe suitable for ultrasonic imaging of turbid water bodies, and the center distance between the square array probes is the size d2 of the single probes.

[0064] In one embodiment, specifically, the frequency of the single probe combined into the planar array probe is 1 MHz, the number is 121 (11 2 ), the chip size is 8.5mm, and the single probe size is 10mm. These probes are combined into Figure 2 and Figure 3 The ultrasonic array probe shown is used for imaging turbid water bodies, and the center distance is 8.5mm.

[0065] Step S2, according to the 2D ultrasonic probe parameters in step S1, setting the imaging space depth below the 2D ultrasonic probe, calculating the effective area of ​​the sound beam generated by the array probe, and determining the imaging area range of the 2D ultrasonic probe in turbid water;

[0066] like Figure 3 As shown, in one embodiment, the specific steps of step S2 are as follows:

[0067] Step S21, through the center element position of the planar array probe, a perpendicular line of the probe plane is drawn along the ultrasonic emission direction as the z-axis, the intersection of the z-axis and the center element plane is taken as the origin, and two directions extending along the array arrangement are taken as the x-axis and the y-axis respectively, so as to define an imaging coordinate system;

[0068] Step S22: based on the imaging coordinate system defined in step S21, set the upper and lower boundary coordinate values ​​of the z-axis of the imaging coordinate system of the array probe to be z min and z max ;

[0069] Step S23, according to the array probe parameters determined in step S1, the directional characteristics of the array in turbid water are calculated, and the half diffusion angle θ of the array in turbid water is calculated using the directional characteristics. -6dBBased on the upper and lower boundary coordinate values ​​of the z-axis set in step S22, the half diffusion angle θ is used. -6dB Calculate the boundary value x of the array probe in the x-axis and y-axis directions in the xy plane min 、x max ,y min and max , as the effective area of ​​the sound beam:

[0070] The directional characteristics of the array along the x-axis and y-axis in turbid water are:

[0071] θ -6dB , where θ -6dB satisfy

[0072] The half diffusion angle of the array in turbid water is:

[0073] The x-axis boundary value of the array probe in the xy plane is:

[0074] The y-axis boundary value of the array probe in the xy plane is:

[0075] Step S24, setting the imaging area range to: length interval [x min ,x max ], the width interval is [y min ,y max ], the height interval is [z min ,z max ].

[0076] In one embodiment, the upper and lower boundary coordinate values ​​of the z-axis are set to [400mm, 500mm], and the half-diffusion angle of the array probe is calculated to be 6° according to the array probe parameters in step 1. The imaging area ranges from [-85mm, 85mm] in length, [-85mm, 85mm] in width, and [400mm, 500mm] in height.

[0077] Step S3, performing three-dimensional ultrasonic imaging detection on the detected target in turbid water according to the 2D ultrasonic probe parameters in step S1 and the imaging area range in step S2, eliminating ultrasonic image noise by using the half-diffusion angle of the array element acoustic beam, and performing ultrafast frame rate three-dimensional ultrasonic imaging of short-range small targets in turbid water at 20 Hz or above;

[0078] In one embodiment, the three-dimensional ultrasound imaging takes the three-dimensional ultrasound full-focus imaging algorithm as an example, and the specific steps of step S3 are as follows:

[0079] Step S31: perform N2 Transmit times, receive N times each time 2 The echo data of array elements is obtained 2 ×N 2 Groups of data, each of which is an A-scan signal;

[0080] Step S32, dividing the imaging area obtained in step S2 into pixels, where the intervals of the pixels in the x, y and z directions are dx, dy and dz respectively;

[0081] Step S33, calculate the transit time of each transmitting-receiving array element pair at each imaging pixel point, and use the array element acoustic beam half-diffusion angle to optimize the imaging algorithm. If a certain pixel point is within the half-diffusion space of a certain transmitting-receiving array element pair, delay the echo data by the transit time obtained in step S32, otherwise discard the contribution of the transmitting-receiving array element pair to the imaging amplitude of the pixel point to avoid introducing noise signals, and perform ultra-fast frame rate three-dimensional ultrasonic full-focus imaging of targets under turbid water bodies. (If three-dimensional ultrasonic imaging uses three-dimensional ultrasonic plane wave imaging, it only needs to transmit M times to the array probe, and receive N times each time. 2 The echo data of array elements is obtained by 2 The subsequent algorithm processing steps are the same. If synthetic aperture three-dimensional ultrasound imaging is used, it is only necessary to perform self-transmission and self-reception of a single probe in the array probe in turn to obtain N 2 The subsequent algorithm processing steps are the same for each group of echo data)

[0082] In one embodiment, the obtained three-dimensional ultrasonic fully focused imaging result of the turbid underwater target is as follows: Figure 4 As shown, the intervals between the imaging pixels in the ROI area in the x, y, and z directions are 1 mm, 1 mm, and 1 mm, respectively.

[0083] Step S4, using a target detection algorithm to perform target detection on the optimized denoised three-dimensional ultrasound fully focused image in step S3. The three-dimensional ultrasound image obtained based on step S3 is essentially a set of three-dimensional matrix data. There is a maximum amplitude pixel at the location of the detected target. By finding the maximum amplitude pixel, the index [L, M, N] of the maximum amplitude pixel in the three-dimensional matrix is ​​inferred, thereby calculating the three-dimensional spatial coordinates of the target in the coordinate system of the array probe. The calculation methods are x=x min +Ldx,y=y min +Mdy,z=z min +Ndz;

[0084] Step S5: according to the position and posture relationship matrix of the array probe in the robot arm base coordinate system The three-dimensional space coordinates (x, y, z) of the target in the array coordinate system obtained in step S4 are converted into the three-dimensional space coordinates (x w ,y w ,z w ), the calculation relationship is: And set the detection target posture and the robot end effector posture information to be the same as Get the position information T of the target object in the robot base coordinate system:

[0085]

[0086] Step S6, according to the position and posture information of the robot arm in the base coordinate system, the Euler angle of the robot arm end posture is calculated, and the rotation angle of each joint of the robot arm is calculated according to the inverse kinematics of the robot;

[0087] In one embodiment, the specific steps of step S6 are as follows:

[0088] Step S61, according to the posture information T in the robot base coordinate system, the Euler angle Q of the robot end posture is calculated:

[0089]

[0090] Step S62, convert the position information T into the rotation angle θ of each joint of the six-degree-of-freedom manipulator through robot inverse kinematics:

[0091] According to the following formula:

[0092]

[0093]

[0094] Obtain the rotation angle θ of each joint of the six-degree-of-freedom robotic arm:

[0095]

[0096] Where r and p represent the rotation operator and translation operator respectively;

[0097] Step S7, according to the initial joint angle θ' of the robot arm, calculate the angle deviation Δθ required for each joint of the robot arm to reach the desired feature point, perform linear fitting on the angle deviation, convert it into a control signal and transmit it to the robot arm controller to drive the robot arm to complete the target tracking task. The specific calculation formula is as follows:

[0098]

[0099] The linear fitting function is:

[0100] Δθ=ωt,

[0101] Where t is time and ω is angular velocity.

[0102] The present invention provides a robotic arm acoustic visual servo system based on three-dimensional ultrasonic imaging in turbid water. Ultrasonic imaging replaces optical camera imaging, and the ultrasonic imaging result is used as input to perform servo control on the robotic arm in turbid water, which solves the problem of poor visual servo effect or visual servo error in traditional turbid water. It can be widely used in submarine operation scenarios, such as submarine pipeline weld inspection, and has broad market prospects. The system specifically includes:

[0103] An imaging range control module is used to adjust the 2D ultrasonic probe parameters according to the characteristics of turbid water; and to set the imaging space depth according to the probe parameters to obtain the imaging range;

[0104] The three-dimensional ultrasonic imaging detection module is used to perform three-dimensional ultrasonic imaging detection on the target based on the ultrasonic probe parameters and imaging range; the ultrasonic image noise is eliminated by using the half-diffusion angle of the array element sound beam to obtain the three-dimensional ultrasonic imaging result;

[0105] The target detection module is used to perform target detection on the 3D ultrasound imaging results and obtain the spatial coordinates and position information of the target in the 2D ultrasound probe coordinate system;

[0106] The acoustic-visual conversion module is used to adjust the spatial coordinates and posture information of the target in the 2D ultrasound probe coordinate system to the robotic arm coordinate system; calculate the Euler angle of the robotic arm end posture and the rotation angle of each joint of the robotic arm through the spatial coordinates and posture information;

[0107] The robot arm servo module is used to obtain the angle deviation through the rotation angle of each joint and the initial angle of the robot arm, and perform linear fitting, which is converted into a control signal and transmitted to the robot arm controller to drive the robot arm to complete the target tracking task.

[0108] like Figure 5 As shown, an embodiment of the present invention provides a robotic arm acoustic visual servo device for turbid water bodies, including a memory and one or more processors. The memory stores executable codes. When the processor executes the executable codes, it is used to implement a robotic arm acoustic visual servo method for turbid water bodies in the above embodiment.

[0109] For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the unit described as a separate component may or may not be physically separated, and the component displayed as a unit may or may not be a physical unit, that is, it may be located in one place, or it may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of the present invention. Ordinary technicians in this field can understand and implement it without paying creative work. For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the unit described as a separate component may or may not be physically separated, and the component displayed as a unit may or may not be a physical unit, that is, it may be located in one place, or it may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of the present invention. Ordinary technicians in this field can understand and implement it without paying creative work.

[0110] An embodiment of the present invention further provides a computer-readable storage medium having a program stored thereon. When the program is executed by a processor, a robotic arm acoustic visual servoing method for turbid water bodies in the above embodiment is implemented.

[0111] The computer-readable storage medium may be an internal storage unit of any device with data processing capability described in any of the aforementioned embodiments, such as a hard disk or a memory. The computer-readable storage medium may also be an external storage device of any device with data processing capability, such as a plug-in hard disk, a smart media card (SMC), an SD card, a flash card, etc. equipped on the device. Furthermore, the computer-readable storage medium may also include both an internal storage unit and an external storage device of any device with data processing capability. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capability, and may also be used to temporarily store data that has been output or is to be output.

[0112] The present invention also provides a computer program product, including a computer program / instruction, which, when executed by a processor, implements the robotic arm acoustic visual servoing method for turbid water bodies.

[0113] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0114] The above is a description of a specific embodiment of the specification. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recorded in the claims can be performed in an order different from that in the embodiments and still achieve the desired results. In addition, the processes depicted in the drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0115] The terms used in one or more embodiments of this specification are only for the purpose of describing specific embodiments, and are not intended to limit one or more embodiments of this specification. The singular forms of "a", "a", "an", and "the" used in one or more embodiments of this specification and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0116] It should be understood that although the terms first, second, third, etc. may be used to describe various information in one or more embodiments of this specification, these information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of one or more embodiments of this specification, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0117] The above are only preferred embodiments of one or more embodiments of this specification, and are not intended to limit one or more embodiments of this specification. Any modifications made within the spirit and principles of one or more embodiments of this specification are not intended to limit this specification.

[0118] The above embodiments are used to illustrate the present invention rather than to limit the present invention. Any modification and change made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A robotic arm acoustic visual servo method for turbid water bodies, characterized in that: include: S1. Adjust the parameters of the 2D ultrasonic probe according to the characteristics of the turbid water body; the 2D ultrasonic probe is a planar array probe, and the planar array probe arranges N 2 The single probes are combined into a square array probe suitable for ultrasonic imaging of turbid water, and the center distance is the single probe size d2; and the imaging space depth is set according to the probe parameters to obtain the imaging range; the adjustment of the 2D ultrasonic probe parameters according to the characteristics of the turbid water body is specifically as follows: according to the longitudinal wave sound velocity v and attenuation characteristics of the turbid water body, the single probe frequency f and the number N in the probe are determined 2 , chip size d1 and single probe size d2, I = I0·e -αx , where I is the sound intensity at distance x, I0 is the initial intensity of the sound wave, α is the attenuation coefficient, x is the distance the sound wave propagates, the frequency f of the single probe in the probe is lower than that in normal waters, and the number N 2 Larger than normal waters; The imaging space depth is set according to the probe parameters to obtain the imaging range, which specifically includes: Through the center array element position of the 2D ultrasound probe, the perpendicular line of the probe plane along the ultrasonic emission direction is taken as the z-axis, the intersection of the z-axis and the center array element plane is taken as the origin, and the two directions extending along the array arrangement are taken as the x-axis and y-axis respectively to define the imaging coordinate system; the upper and lower boundary coordinate values ​​of the z-axis of the 2D ultrasound probe imaging coordinate system are set as z min and z max ; According to the probe parameters, the directional characteristics of the array in turbid water are calculated, and the half diffusion angle θ of the array in turbid water is calculated using the directional characteristics. -6dB , based on the upper and lower boundary coordinates of the z-axis, using the half diffusion angle θ -6dB Calculate the boundary value x of the array probe in the x-axis and y-axis directions in the xy plane min 、x max ,y min and max , as the effective area of ​​the sound beam, determine the imaging area range of the 2D ultrasonic probe in the turbid water body; The directional characteristics of the 2D ultrasonic probe array along the x-axis and y-axis in turbid water are: The half diffusion angle of the 2D ultrasonic probe array in turbid water is: θ -6dB , where θ -6dB satisfy Where v represents the longitudinal wave speed in turbid water; The x-axis boundary value of the 2D ultrasound probe in the xy plane is: The y-axis boundary value of the 2D ultrasound probe in the xy plane is: S2. Perform three-dimensional ultrasonic imaging detection on the target based on the ultrasonic probe parameters and imaging range; use the array element acoustic beam half-diffusion angle optimization imaging algorithm to eliminate ultrasonic image noise and obtain three-dimensional ultrasonic imaging results; S3, performing target detection on the three-dimensional ultrasound imaging result to obtain the position information of the target in the 2D ultrasound probe imaging coordinate system; S4, adjusting the position and posture information of the target in the 2D ultrasound probe imaging coordinate system to the robot arm base coordinate system; calculating the Euler angle of the robot arm end posture and the rotation angle of each joint of the robot arm through the position and posture information; S5. The rotation angle deviation is obtained through the rotation angle of each joint and the initial angle of the robot arm, and a linear fitting is performed. The result is converted into a control signal and transmitted to the robot arm controller to drive the robot arm to complete the target tracking task.

2. The method for robotic arm acoustic visual servoing for turbid water according to claim 1, characterized in that: The three-dimensional ultrasound imaging result in S2 includes three-dimensional ultrasound full-focus imaging, specifically: performing N 2 Transmit times, receive N times each time 2 The echo data of array elements is obtained 2 ×N 2 Groups of data, each of which is an A-scan signal; The imaging area is divided into pixels, and the intervals of the pixels in the x, y, and z directions are dx, dy, and dz respectively; The transit time of each transmitting-receiving array element pair at each imaging pixel is calculated, and the imaging algorithm is optimized using the semi-diffusion angle of the array element acoustic beam. If a certain pixel is within the semi-diffusion space of a certain transmitting-receiving array element pair, the echo data is delayed and superimposed by the transit time. Otherwise, the contribution of the transmitting-receiving array element pair to the imaging amplitude of the pixel is discarded to avoid the introduction of noise signals. Ultrafast frame rate three-dimensional ultrasonic full-focusing imaging of targets in turbid water bodies with a frame rate of more than 20 Hz is performed.

3. The method for robotic arm acoustic visual servoing for turbid water according to claim 1, characterized in that: S3 specifically includes: finding the maximum-amplitude pixel point in the three-dimensional ultrasound imaging result, and inferring the index [L, M, N] of the maximum-amplitude pixel point in the three-dimensional matrix, thereby calculating the three-dimensional spatial coordinates of the target in the 2D ultrasound probe imaging coordinate system, and the calculation methods are respectively x=x min +Ldx,y=y min +Mdy,z=z min +Ndz; where x min ,y min and z min They are respectively the lower boundary coordinate values ​​of the x, y, and z axes of the 2D ultrasound probe imaging coordinate system.

4. The method for robotic arm acoustic visual servoing for turbid water according to claim 1, characterized in that: The S4 specifically includes: according to the position relationship matrix of the 2D ultrasonic probe in the robot arm base coordinate system The three-dimensional spatial coordinates (x, y, z) of the target in the 2D ultrasound probe imaging coordinate system are converted into the three-dimensional spatial coordinates (x w ,y w ,z w ), the calculation relationship is: And set the detection target posture and the robot end effector posture information to be the same as Get the target's position information T in the robot arm base coordinate system: According to the pose information T in the robot base coordinate system, the Euler angle Q of the robot end pose is calculated: The posture information T is converted into the rotation angle θ of each joint of the six-degree-of-freedom manipulator through the robot inverse kinematics; according to the following formula: Obtain the rotation angle θ of each joint of the six-degree-of-freedom robotic arm: Where r and p represent the rotation operator and translation operator respectively.

5. The method for robotic arm acoustic visual servoing for turbid water according to claim 1, characterized in that: The S5 specifically includes: according to the initial joint angle θ' of the robot arm, calculating the rotation angle deviation Δθ required for each joint of the robot arm to reach the desired characteristic point, performing linear fitting on the rotation angle deviation, converting it into a control signal and transmitting it to the robot arm controller to drive the robot arm to complete the target tracking task. The specific calculation formula is as follows: The linear fitting function is: Δθ=ωt, Where t is time and ω is angular velocity.

6. A robotic arm acoustic visual servo system for turbid water bodies implemented by the method according to any one of claims 1 to 5, characterized in that: include: An imaging range control module is used to adjust the 2D ultrasonic probe parameters according to the characteristics of the turbid water body; and to set the imaging space depth according to the probe parameters to obtain the imaging range; The three-dimensional ultrasonic imaging detection module is used to perform three-dimensional ultrasonic imaging detection on the target based on the ultrasonic probe parameters and imaging range; the ultrasonic image noise is eliminated by optimizing the imaging algorithm of the half-diffusion angle of the array element sound beam to obtain the three-dimensional ultrasonic imaging result; The target detection module is used to perform target detection on the 3D ultrasound imaging results and obtain the position information of the target in the 2D ultrasound probe imaging coordinate system; The acoustic-visual conversion module is used to adjust the target's posture information in the 2D ultrasound probe imaging coordinate system to the robotic arm base coordinate system; calculate the Euler angle of the robotic arm's end posture and the rotation angle of each joint of the robotic arm through the posture information; The robot arm servo module is used to obtain the angle deviation through the rotation angle of each joint and the initial angle of the robot arm, and perform linear fitting, which is converted into a control signal and transmitted to the robot arm controller to drive the robot arm to complete the target tracking task.

7. A robotic arm acoustic visual servo device for turbid water, comprising a memory and one or more processors, wherein the memory stores executable code, characterized in that: When the processor executes the executable code, it is used to implement a robotic arm acoustic visual servoing method for turbid water bodies according to any one of claims 1 to 5.

Images