Control method and device of mechanical arm, mechanical arm and readable storage medium

Abstract

The present invention relates to the field of robot control technology. The present invention provides a control method and device for a robot arm, a robot arm, and a readable storage medium. The method comprises: determining the force component in the current moving direction of the robot arm according to the contact force detection result fed back by the force sensor; determining the target moving speed and / or the target contact position of the robot arm according to the force component and the current moving direction; and controlling the robot arm to perform ultrasonic scanning according to the target moving speed and / or the target contact position. The contact force between the robot end and the environment is adjusted by adjusting the speed and / or position of the robot, and the force deviation is corrected, so that the robot has a certain degree of compliance during the scanning process, thereby improving the comfort of the inspector during the ultrasonic scanning process.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a control method and device for a robot arm, a robot arm and a readable storage medium. Background Art

[0002] The new breast ultrasound scan usually uses a robot to replace the ultrasound doctor's handheld ultrasound equipment to operate, which has the advantages of fast scanning speed and labor saving.

[0003] In order to ensure that the images obtained by the robot scanning are clear enough, the traditional robot scanning method requires constant force control of the robot arm, that is, controlling the robot arm to scan the human body according to a preset trajectory at a constant speed and force.

[0004] However, during breast ultrasound scanning, the robotic arm needs to be in direct contact with the human body. Since the robotic arm is often very rigid and has constant force, it is easy to cause discomfort to the person being examined during contact with the human body, and even cause damage to the person being examined in some serious cases. Therefore, the traditional rigid, constant force breast ultrasound scanning method using a robotic arm has the problem of low comfort and safety during the scanning process.

[0005] The above contents are only used to assist in understanding the technical solution of the present invention and do not constitute an admission that the above contents are prior art. Summary of the invention

[0006] The main purpose of the present invention is to provide a control method for a robotic arm, aiming to solve the problem of low comfort level for inspectors during ultrasonic scanning inspection.

[0007] To achieve the above object, the present invention provides a control method for a robotic arm, the control method for the robotic arm comprising:

[0008] Determining the force component in the current moving direction of the robotic arm according to the contact force detection result fed back by the force sensor;

[0009] Determining a target moving speed of the robotic arm and / or a target contact position according to the force component and the current moving direction;

[0010] The robotic arm is controlled to perform an ultrasonic scanning action according to the target moving speed and / or the target contact position.

[0011] Optionally, before the step of determining the force component in the current moving direction of the robotic arm according to the contact force detection result fed back by the force sensor, the step further includes:

[0012] Obtaining an initial scanning trajectory of the robotic arm;

[0013] The robotic arm is controlled to perform an initial ultrasonic scanning action according to the initial scanning trajectory.

[0014] Optionally, the step of determining a target moving speed and / or a target contact position of the robotic arm according to the force component and the current moving direction comprises:

[0015] Obtaining an expected value of the contact force in the same direction as the force component;

[0016] determining a deviation force between the expected value of the contact force and the force component;

[0017] Based on the admittance transfer function, determining the force deviation amount of the robotic arm according to the deviation force;

[0018] A target contact position of the robot arm is determined according to the force deviation amount.

[0019] Optionally, the step of determining the force deviation of the robotic arm according to the deviation force based on the admittance transfer function comprises:

[0020] Obtaining inertia parameters, and damping parameters and stiffness parameters corresponding to the robotic arm;

[0021] determining an admittance transfer function associated with the robotic arm based on the inertia parameter, the damping parameter, the stiffness parameter, and the deviation force;

[0022] determining a discrete admittance transfer function corresponding to the admittance transfer function;

[0023] The force deviation amount is determined according to the discrete admittance transfer function based on a preset sampling time.

[0024] Optionally, before the step of determining the target moving speed and / or target contact position of the robotic arm according to the force component and the current moving direction, the step further includes:

[0025] Obtaining the current moving speed of the robotic arm, and obtaining the target moving speed and target moving direction;

[0026] The step of determining the target moving speed and / or target contact position of the robotic arm according to the force component and the current moving direction comprises:

[0027] determining an acceleration of the robotic arm according to the force component and the current movement rate;

[0028] Determine an offset rate between the current moving speed and the target moving speed, and determine an offset direction between the current moving direction and the target moving direction;

[0029] The target moving speed is determined according to the offset rate, the offset direction and the acceleration.

[0030] Optionally, the step of controlling the robotic arm to perform an ultrasonic scanning action according to the target moving speed and / or the target contact position includes:

[0031] Determining a control parameter of the robotic arm according to the target moving speed, and / or determining a control parameter of the robotic arm according to the target contact position;

[0032] The robotic arm is controlled to perform the ultrasonic scanning action according to the control parameters.

[0033] Optionally, the control method of the robotic arm further includes:

[0034] Obtaining the current moving position of the robotic arm;

[0035] Determining a movement amount between the current movement position and the target movement position;

[0036] The robot arm is controlled to move toward the target moving position according to the movement amount, wherein during the movement toward the target moving position, the contact force detection result fed back by the force sensor arranged on the end of the robot arm remains unchanged.

[0037] In addition, to achieve the above-mentioned purpose, the present invention further provides a control device for a robotic arm, the control device for the robotic arm comprising:

[0038] An admittance control module, used to determine a target contact position and / or a target moving speed of the robotic arm according to a force detection result fed back by a force sensor at the end of the robotic arm;

[0039] A position control module is used to control the robotic arm to perform an ultrasonic scanning action according to the target contact position and / or the target moving speed.

[0040] In addition, to achieve the above-mentioned purpose, the present invention also provides a robotic arm, which includes: a memory, a processor, and a robotic arm control program stored in the memory and runnable on the processor, and when the robotic arm control program is executed by the processor, the steps of the robotic arm control method as described in any one of the above items are implemented.

[0041] The embodiment of the present invention provides a control method, device, robot arm and computer-readable storage medium for a robot arm, the method comprising: determining the force component in the current moving direction of the robot arm according to the contact force detection result fed back by the force sensor; determining the target moving speed and / or the target contact position of the robot arm according to the force component and the current moving direction; and controlling the robot arm to perform ultrasonic scanning according to the target moving speed and / or the target contact position. The contact force between the robot end and the environment is adjusted by adjusting the speed and / or position of the robot, and the force deviation is corrected, so that the robot has a certain degree of compliance during the scanning process, thereby improving the comfort of the inspector during the ultrasonic scanning process. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 Schematic diagram of the hardware architecture of the robotic arm involved in an embodiment of the present invention;

[0043] Figure 2 It is a schematic flow chart of a first embodiment of a method for controlling a robot arm of the present invention;

[0044] Figure 3 is a block diagram of constant force control based on admittance control in this embodiment;

[0045] Figure 4 This is a detailed flow chart of step S20 in the second embodiment of the method for controlling a robot arm of the present invention;

[0046] Figure 5 This is a detailed flow chart of step S23 in the third embodiment of the method for controlling a robot arm of the present invention;

[0047] Figure 6 is a schematic flow chart of a fourth embodiment of a method for controlling a robot arm of the present invention;

[0048] Figure 7 It is a detailed flow chart of step S50 in the fifth embodiment of the control method of the robot arm of the present invention;

[0049] Figure 8 is a flow chart of a sixth embodiment of a method for controlling a robot arm of the present invention;

[0050] Fig. 9 A schematic diagram of an impedance model of the control method of the robot arm of the present invention;

[0051] Fig.10 FIG. 4 is a schematic diagram of the structure of the control device of the robot arm of the present invention.

[0052] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0053] It should be understood that the exemplary embodiments of the present invention are shown in the accompanying drawings of the present invention, and the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present invention and to fully convey the scope of the present invention to those skilled in the art.

[0054] As an implementation scheme, the execution device corresponding to the control method of the robot arm can be as follows Figure 1 shown.

[0055] The embodiment of the present invention relates to an execution device of a robot arm, which includes: a processor 101, such as a CPU, a memory 102, and a communication bus 103. The communication bus 103 is used to realize connection and communication between these components.

[0056] The memory 102 may be a high-speed RAM memory or a stable memory (non-volatile memory), such as a disk memory. Figure 1 As shown, the memory 102 as a computer-readable storage medium may include a control program for the robotic arm; and the processor 101 may be used to call the control program for the robotic arm stored in the memory 102 and perform the following operations:

[0057] Obtaining an initial scanning trajectory of the robotic arm;

[0058] Controlling the robotic arm to perform an initial ultrasonic scanning action according to the initial scanning trajectory;

[0059] Determining the force component in the current moving direction of the robotic arm according to the contact force detection result fed back by the force sensor;

[0060] Determining a target moving speed of the robotic arm and / or a target contact position according to the force component and the current moving direction;

[0061] The robotic arm is controlled to perform an ultrasonic scanning action according to the target moving speed and / or the target contact position.

[0062] In one embodiment, the processor 101 may be used to call the control program of the robot arm stored in the memory 102, and perform the following operations:

[0063] Obtaining an expected value of the contact force in the same direction as the force component;

[0064] determining a deviation force between the expected value of the contact force and the force component;

[0065] Based on the admittance transfer function, determining the force deviation amount of the robotic arm according to the deviation force;

[0066] A target contact position of the robot arm is determined according to the force deviation amount.

[0067] In one embodiment, the processor 101 may be used to call the control program of the robot arm stored in the memory 102, and perform the following operations:

[0068] Obtaining inertia parameters, and damping parameters and stiffness parameters corresponding to the robotic arm;

[0069] determining an admittance transfer function associated with the robotic arm based on the inertia parameter, the damping parameter, the stiffness parameter, and the deviation force;

[0070] determining a discrete admittance transfer function corresponding to the admittance transfer function;

[0071] The force deviation amount is determined according to the discrete admittance transfer function based on a preset sampling time.

[0072] In one embodiment, the processor 101 may be used to call the control program of the robot arm stored in the memory 102, and perform the following operations:

[0073] Obtaining the current moving speed of the robotic arm, and obtaining the target moving speed and target moving direction;

[0074] The step of determining the target moving speed and / or target contact position of the robotic arm according to the force component and the current moving direction comprises:

[0075] determining an acceleration of the robotic arm according to the force component and the current movement rate;

[0076] Determine an offset rate between the current moving speed and the target moving speed, and determine an offset direction between the current moving direction and the target moving direction;

[0077] The target moving speed is determined according to the offset rate, the offset direction and the acceleration.

[0078] In one embodiment, the processor 101 may be used to call the control program of the robot arm stored in the memory 102, and perform the following operations:

[0079] Determining a control parameter of the robotic arm according to the target moving speed, and / or determining a control parameter of the robotic arm according to the target contact position;

[0080] The robotic arm is controlled to perform the ultrasonic scanning action according to the control parameters.

[0081] In one embodiment, the processor 101 may be used to call the control program of the robot arm stored in the memory 102, and perform the following operations:

[0082] Obtaining the current moving position of the robotic arm;

[0083] Determining a movement amount between the current movement position and the target movement position;

[0084] The robot arm is controlled to move toward the target moving position according to the movement amount, wherein during the movement toward the target moving position, the contact force detection result fed back by the force sensor arranged on the end of the robot arm remains unchanged.

[0085] In traditional breast ultrasound examinations, doctors need to hold ultrasound equipment and operate it manually. This method has a slow scanning speed and requires a lot of experience from ultrasound doctors. The new breast ultrasound scanning method uses robots to replace ultrasound doctors, which has the advantages of fast scanning speed and labor saving. However, if the robot is simply controlled by position mode for scanning, the image obtained is often blurry and of low quality, which seriously affects the ultrasound doctor's reading and judgment. Feedback from experienced ultrasound doctors and a large number of experiments have proved that excellent ultrasound images require constant force control by robots, but the existing constant force control method directly used in breast scanning is not ideal, with large steady-state errors and slow tracking force speed. The breast ultrasound scanning scene is quite special. The robot acts directly on the human body. The mechanical arm with pure position control is very rigid. A small position error will generate a lot of pressure. The pressure directly acts on the human breast, which will cause breast deformation and even threaten the safety of women's lives. In this scenario, the use of admittance control can improve the control effect.

[0086] Based on the hardware architecture of the control device of the robotic arm based on the robot control technology, an embodiment of the control method of the robotic arm of the present invention is proposed.

[0087] Reference Figure 2 In a first embodiment, the control method of the robot arm comprises the following steps:

[0088] Step S10, obtaining the initial scanning trajectory of the robotic arm;

[0089] Step S20, controlling the robotic arm to perform an initial ultrasonic scanning action according to the initial scanning trajectory;

[0090] In this embodiment, the robot controls the robotic arm to perform ultrasonic scanning on the human body based on the position mode. In the position mode, the robot determines the initial scanning trajectory of the robotic arm during the ultrasonic scanning process according to the preset scanning logic. The end of the robotic arm contacts the body of the human body, moves according to the initial scanning trajectory, and performs the scanning action.

[0091] Step S30, determining the force component in the current moving direction of the robot arm according to the contact force detection result fed back by the force sensor;

[0092] In this embodiment, the admittance control is impedance control based on the position mode. A force sensor is installed at the end of the robot to measure the end contact force, realize compliant control, avoid brute force directly acting on the human body, and realize constant force control. A force sensor is provided at the end of the robot arm, which is communicatively connected to the robot arm control device. The force sensor can be a multi-dimensional force sensor (such as a six-dimensional force sensor). During the process of the robot arm performing ultrasonic scanning, the force component of the output force of the robot arm in the moving direction according to the ultrasonic scanning trajectory is determined according to the contact force detection result fed back in real time on the multi-dimensional force sensor. According to the force component, the magnitude of the force at the contact part between the robot arm and the human body can be determined in real time, and the constant force control of the robot arm during the ultrasonic scanning process can be maintained.

[0093] Step S40, determining a target moving speed and / or a target contact position of the robotic arm according to the force component and the current moving direction;

[0094] Step S50: controlling the robot arm to perform an ultrasonic scanning action according to the target moving speed and / or the target contact position.

[0095] In this embodiment, based on the admittance control principle, the target moving position of the robot arm can be obtained according to the force component and the current moving direction of the robot arm. When the robot is subjected to an external force, it will deviate from the original trajectory to comply with the external force. The implementation method is to generate a new desired position ΔX at the admittance controller, and under the action of the internal position closed-loop controller, the control target movement from X to ΔX is achieved.

[0096] Reference Figure 3 , Figure 3 is a block diagram of constant force control based on admittance control in this embodiment. 0 is the expected force value, that is, the target value to be tracked; ΔX = [Δx, Δy, Δα, Δβ, Δγ,], which represents the offset of the robot in each direction; ΔF = [ΔF X ,ΔF Y ,ΔF z ,ΔT x ,ΔT y ,ΔT z ], indicating the deviation signal of the contact force in each direction of the robot. 0The expected force value is a pressure value (referenced as 15-20N) that is adjusted so that the inspector will not feel uncomfortable during the ultrasonic scanning process of the robot. Based on the ΔF obtained by the force sensor, according to the admittance control principle, the position controller controls the manipulator to deflect according to the offset ΔX. After the offset, the contact force between the contact part of the manipulator and the inspector changes to the expected value F. 0 , the value on the force sensor also changes accordingly to F 0 , thereby achieving the purpose of measuring the contact between the robot and the environment, designing a controller based on the deviation between the contact force and the ideal value, and then adjusting the contact force between the robot end and the environment by adjusting the robot's speed or position to correct the force deviation.

[0097] In some embodiments, during the ultrasonic scanning process of the robot, the contact force data fed back by the force sensor at the end of the robotic arm is detected in real time, and the contact force data is matched with a preset expected force value at a certain frequency to ensure that the contact force and the expected force value are the same as much as possible during the ultrasonic scanning process. The contact force is indirectly changed through the admittance control in the system, so that the robot has a certain degree of compliance during the scanning process.

[0098] In the technical solution provided in this embodiment, a force sensor is installed at the end of the robotic arm, and the target moving speed and / or the target contact position of the robotic arm are determined according to the component force in the same direction of movement of the robotic arm fed back by the force sensor, and the robotic arm is controlled to perform ultrasonic scanning action according to the target moving speed and / or the target contact position, so that the robot has a certain degree of compliance during the scanning process, thereby improving the comfort of the inspector during the ultrasonic scanning inspection process.

[0099] Reference Figure 4 In the second embodiment, based on the first embodiment, step S20 includes:

[0100] Step S21, obtaining an expected value of the contact force in the same direction as the force component;

[0101] Step S22, determining a deviation force between the expected value of the contact force and the force component;

[0102] Step S23, determining the force deviation of the robotic arm according to the deviation force based on the admittance transfer function;

[0103] Step S24: determining a target contact position of the robot arm according to the force deviation.

[0104] Optionally, this embodiment provides a method for controlling the displacement of the end when the force sensor senses an external force. During the ultrasonic scanning process, since the detection part of the human body is not planar but has a certain three-dimensional structure, when the robotic arm moves according to the ultrasonic scanning trajectory, it is necessary to change the contact position between the robotic arm and the contact part according to the distance between the corresponding contact part and the robotic arm torque, so as to avoid the discomfort and injury caused by excessive pressure on the human body during the ultrasonic scanning process.

[0105] In some embodiments, the inspector lies flat on the inspection table. When the robotic arm is in the scanning process (such as scanning from the abdomen to the chest of the human body), since the chest is closer to the torque of the robotic arm relative to the abdomen, if the original torque is maintained to scan the chest, the pressure between the end of the robotic arm and the contact part will increase, causing discomfort to the inspector. In order to avoid excessive pressure, the force parameters of the end of the robotic arm are obtained in real time during the scanning process, and the force parameters are matched with the expected contact force. When there is a deviation between the two, the torque of the robotic arm is changed according to the deviation through the admittance transfer function to change the target contact position of the robotic arm according to the torque. Thereby ensuring that the contact force at the end of the robotic arm is always consistent with the expected value, avoiding discomfort and damage to the human body due to excessive pressure during the ultrasonic scanning process.

[0106] In the technical solution provided in this embodiment, the target contact position of the robotic arm is changed by changing the torque of the robotic arm, thereby ensuring that the contact force at the end of the robotic arm is always consistent with the expected value, thereby avoiding excessive pressure on the human body during ultrasonic scanning, causing discomfort or even injury.

[0107] Reference Figure 5 In the third embodiment, based on the second embodiment, step S23 includes:

[0108] Step S231, obtaining inertia parameters, and damping parameters and stiffness parameters corresponding to the robotic arm;

[0109] Step S232, determining an admittance transfer function associated with the robotic arm according to the inertia parameter, the damping parameter, the stiffness parameter and the deviation force;

[0110] Step S233, determining a discrete admittance transfer function corresponding to the admittance transfer function;

[0111] Step S234: determining the force deviation according to the discrete admittance transfer function based on a preset sampling time.

[0112] Optionally, admittance control is the control algorithm of the present invention. Admittance control is to establish the relationship between the robot end displacement and the contact force by equivalent the robot end force position control to the "spring-mass-damping" model, and to adjust the force by arbitrarily adjusting the damping and stiffness parameters, and then the relationship between the robot end position and the contact force. Generally, the impedance model is set as a second-order differential equation, as shown below:

[0113]

[0114] Among them, M d is the inertia parameter, B d is the damping parameter, K d is the rigidity parameter, X r -X) is the end displacement;

[0115] Without considering M d In this case, the impedance model is simplified to:

[0116]

[0117] After the above formula is transformed by the pull method, we get:

[0118]

[0119] H is the system admittance transfer function, B d and K d are the admittance parameters, and admittance control is achieved by changing these two parameters;

[0120] Discretize this transfer function using first-order backward difference:

[0121] make Where T is the sampling time;

[0122] Substituting into the transfer function we get:

[0123]

[0124] Further deduction yields:

[0125]

[0126] Finally we get:

[0127]

[0128] By adjusting k 1 and k 2 The admittance control of the robotic arm can be completed.

[0129] In the technical solution provided in this embodiment, the admittance transfer function of the robotic arm is determined by inertia parameters, damping parameters and stiffness parameters corresponding to the robotic arm, and deviation force, and based on the model constructed by the admittance transfer function, the damping and stiffness parameters of the robotic arm are adjusted to adjust the force to achieve compliant control of the robotic arm.

[0130] Reference Figure 6 In a fourth embodiment, based on any embodiment, before step S20, the method further includes:

[0131] Step S60, obtaining the current moving speed of the robot arm, and obtaining the target moving speed and target moving direction;

[0132] The step S20 comprises:

[0133] Step S25, determining the acceleration of the robotic arm according to the force component and the current moving speed;

[0134] Step S26, determining an offset rate between the current moving speed and the target moving speed, and determining an offset direction between the current moving direction and the target moving direction;

[0135] Step S27, determining the target moving speed according to the offset rate, the offset direction and the acceleration.

[0136] Optionally, in this embodiment, a method for controlling the end moving speed when the force sensor senses an external force is provided. During the scanning process of the robot arm, the robot arm will perform trajectory scanning at a certain initial speed, and the initial speed includes an initial rate and an initial direction. Since the robot arm needs to be in real-time contact with the human body during the scanning process, different scanning positions have different requirements for the moving speed and moving direction of the robot arm. In this embodiment, the moving speed and moving direction corresponding to different scanning positions are fixed preset values, that is, the expected moving speed and the expected moving direction. When the robot arm needs to change the moving speed and moving direction during the scanning process according to the scanning trajectory, the acceleration a is determined by the force component F and the current moving speed of the robot arm, and the speed difference between the target moving speed and the current moving speed and the direction angle difference between the target moving direction and the current offset direction are determined. Based on the offset rate, the offset direction and the acceleration, the target moving speed of the robot arm after the change can be determined, wherein the target moving speed includes the target moving speed and the target moving direction of the robot arm, thereby controlling the robot arm to change the speed and direction according to the scanning trajectory.

[0137] In the technical solution provided in this embodiment, the compliant control of the robot is achieved by matching the guiding force with the speed and acceleration of the end of the robotic arm.

[0138] Reference Figure 7In a fifth embodiment, based on any one of the embodiments, step S50 includes:

[0139] Step S51, determining the control parameters of the robotic arm according to the target moving speed, and / or determining the control parameters of the robotic arm according to the target contact position;

[0140] Step S52: controlling the robotic arm to perform the ultrasonic scanning action according to the control parameters.

[0141] Optionally, since speed-based / displacement-based control of the robotic arm are two different control methods, the resulting control parameters are also different. The method for determining the target moving speed and the target contact position has been given in the previous text. Therefore, in this embodiment, if the robotic arm is controlled based on the moving speed, the acceleration parameter and the target moving speed parameter of the end of the robotic arm are determined according to the feedback force data of the force sensor for ultrasonic scanning; if the robotic arm is controlled based on displacement, the force parameter of the robotic arm is determined according to the feedback force data, and then the target contact position of the robotic arm is determined. It should be emphasized that whether the admittance control is based on speed or displacement, the contact force of the robotic arm with the target body part during the scanning process can be consistent with the expected contact force, thereby avoiding discomfort and injury to the human body caused by excessive pressure during the ultrasonic scanning process.

[0142] Optionally, the robot arm can also be controlled based on speed and displacement at the same time to achieve more precise robot arm control.

[0143] In the technical solution provided in this embodiment, the control parameters of the robotic arm are determined by the target movement speed and / or the target contact position, so that the contact force of the robotic arm with the target body part during the scanning process is consistent with the expected contact force, thereby avoiding excessive pressure on the human body during the ultrasonic scanning process, causing discomfort and injury.

[0144] Reference Figure 8 In a sixth embodiment, based on any one of the embodiments, the control method of the robot arm further includes:

[0145] Step S70, obtaining the current moving position of the robotic arm;

[0146] Step S80, determining the movement amount between the current moving position and the target moving position;

[0147] Step S90: controlling the robot arm to move toward the target position according to the movement amount.

[0148] Optionally, the control method of the robot arm can also be an impedance control method. The core idea of ​​the impedance control method is to control the joint force so that the end of the robot generates impedance force according to the position deviation generated by the end of the robot. It is a control method based on the dynamic relationship between force and speed or position. By establishing a dynamic relationship between the robot movement and the external force, the robot movement is adjusted, thereby achieving dynamic adjustment between the robot and the environment.

[0149] For example, reference Fig. 9 , Fig. 9 It is a schematic diagram of an impedance model. The dynamic equation of the impedance control method is:

[0150]

[0151] Where X is the displacement, M d For quality, B d is the damping, K d is the stiffness, F is the force;

[0152] By Laplace transforming the above dynamic equation, we can get:

[0153] (M d S 2 +B d S+K d )X(s)=F(s)

[0154] Since impedance is defined as Z(s) = F(s) / X(s), admittance is defined as the inverse of impedance:

[0155] Y(s)=Z -1 (s) = X(s) / F(s)

[0156] Since ΔX=YΔF, a displacement change ΔX can be generated by a force change ΔF.

[0157] In addition, refer to Fig.10 This embodiment further provides a control device for a robotic arm, the control device for the robotic arm comprising:

[0158] The admittance control module 100 is used to determine the target contact position and / or target moving speed of the robotic arm according to the force detection result fed back by the force sensor at the end of the robotic arm;

[0159] The position control module 200 is used to control the robot arm to perform ultrasonic scanning according to the target contact position and / or the target moving speed.

[0160] In addition, the present invention also provides a robotic arm, comprising: a memory, a processor, and a robotic arm control program stored in the memory and runnable on the processor, wherein the robotic arm control program, when executed by the processor, implements each step of the robotic arm control method as described in any one of the above items.

[0161] In addition, the present invention also provides a computer-readable storage medium, which stores a control program for a robotic arm. When the control program for the robotic arm is executed by a processor, the various steps of the control method for the robotic arm described in the above embodiment are implemented.

[0162] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article 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, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0163] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a computer-readable storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for enabling a terminal device (which can be a mobile phone, computer, server, or network device, etc.) to execute the methods described in each embodiment of the present invention.

[0164] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for controlling a robotic arm, characterized in that: Applied to a robot arm control device, the robot arm control device is used to control the robot arm to perform ultrasonic scanning, the end of the robot arm is provided with a force sensor that is communicatively connected to the robot arm control device, and the steps of the robot arm control method include: Determining the force component in the current moving direction of the robotic arm according to the contact force detection result fed back by the force sensor; determining a target moving speed and / or a target contact position of the robotic arm according to the force component and the current moving direction; The robotic arm is controlled to perform an ultrasonic scanning action according to the target moving speed and / or the target contact position.

2. The control method of the robot arm according to claim 1, characterized in that: Before the step of determining the force component in the current moving direction of the robot arm according to the contact force detection result fed back by the force sensor, the method further includes: Obtaining an initial scanning trajectory of the robotic arm; The robotic arm is controlled to perform an initial ultrasonic scanning action according to the initial scanning trajectory.

3. The control method of the robot arm according to claim 1, characterized in that: The step of determining the target moving speed and / or target contact position of the robotic arm according to the force component and the current moving direction comprises: Obtaining an expected value of the contact force in the same direction as the force component; determining a deviation force between the expected value of the contact force and the force component; Based on the admittance transfer function, determining the force deviation amount of the robotic arm according to the deviation force; A target contact position of the robot arm is determined according to the force deviation amount.

4. The control method of the robot arm according to claim 3, characterized in that: The step of determining the force deviation of the robotic arm according to the deviation force based on the admittance transfer function comprises: Obtaining inertia parameters, and damping parameters and stiffness parameters corresponding to the robotic arm; determining an admittance transfer function associated with the robotic arm based on the inertia parameter, the damping parameter, the stiffness parameter, and the deviation force; determining a discrete admittance transfer function corresponding to the admittance transfer function; The force deviation amount is determined according to the discrete admittance transfer function based on a preset sampling time.

5. The control method of the robot arm according to claim 1, characterized in that: Before the step of determining the target moving speed and / or target contact position of the robotic arm according to the force component and the current moving direction, the step further includes: Obtaining the current moving speed of the robotic arm, and obtaining the target moving speed and target moving direction; The step of determining the target moving speed and / or target contact position of the robotic arm according to the force component and the current moving direction comprises: determining an acceleration of the robotic arm according to the force component and the current movement rate; Determine an offset rate between the current moving speed and the target moving speed, and determine an offset direction between the current moving direction and the target moving direction; The target moving speed is determined according to the offset rate, the offset direction and the acceleration.

6. The control method of a robot arm according to claim 1, characterized in that: The step of controlling the robotic arm to perform an ultrasonic scanning action according to the target moving speed and / or the target contact position comprises: Determining a control parameter of the robotic arm according to the target moving speed, and / or determining a control parameter of the robotic arm according to the target contact position; The robotic arm is controlled to perform the ultrasonic scanning action according to the control parameters.

7. The control method of a robot arm according to claim 1, characterized in that: The control method of the robotic arm further includes: Obtaining the current moving position of the robotic arm; Determining a movement amount between the current movement position and the target movement position; The robot arm is controlled to move toward the target moving position according to the movement amount, wherein during the movement toward the target moving position, the contact force detection result fed back by the force sensor arranged on the end of the robot arm remains unchanged.

8. A control device for a robotic arm, characterized in that: The robotic arm control device is used to control the robotic arm to perform ultrasonic scanning, and the robotic arm control device includes: A communication module, used for communicating with a force sensor disposed at the end of the mechanical arm; an admittance control module, configured to determine a force component in a current moving direction of the robotic arm according to a force detection result fed back by a force sensor at the end of the robotic arm, and to determine a target contact position and / or a target moving speed of the robotic arm according to the force component and the current moving direction; A position control module is used to control the robotic arm to perform an ultrasonic scanning action according to the target contact position and / or the target moving speed.

9. A robotic arm, characterized in that: The robotic arm comprises: a memory, a processor, and a control program of the robotic arm stored in the memory and executable on the processor. When the control program of the robotic arm is executed by the processor, the steps of the control method of the robotic arm as described in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a control program for the robotic arm, and when the control program for the robotic arm is executed by the processor, the steps of the method for controlling the robotic arm as described in any one of claims 1 to 7 are implemented.

Images