CN115179279B - Control method and device of mechanical arm, mechanical arm and readable storage medium - Google Patents
Control method and device of mechanical arm, mechanical arm and readable storage medium Download PDFInfo
- Publication number
- CN115179279B CN115179279B CN202210702089.0A CN202210702089A CN115179279B CN 115179279 B CN115179279 B CN 115179279B CN 202210702089 A CN202210702089 A CN 202210702089A CN 115179279 B CN115179279 B CN 115179279B
- Authority
- CN
- China
- Prior art keywords
- robotic arm
- force
- mechanical arm
- target
- determining
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 90
- 230000033001 locomotion Effects 0.000 claims abstract description 53
- 230000009471 action Effects 0.000 claims abstract description 25
- 238000001514 detection method Methods 0.000 claims abstract description 18
- 230000006870 function Effects 0.000 claims description 28
- 238000012546 transfer Methods 0.000 claims description 28
- 238000013016 damping Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 12
- 238000004891 communication Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 abstract description 35
- 230000001276 controlling effect Effects 0.000 description 18
- 238000010586 diagram Methods 0.000 description 9
- 238000006073 displacement reaction Methods 0.000 description 9
- 210000000481 breast Anatomy 0.000 description 8
- 230000008859 change Effects 0.000 description 5
- 238000002604 ultrasonography Methods 0.000 description 4
- 210000000038 chest Anatomy 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 210000001015 abdomen Anatomy 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 210000005075 mammary gland Anatomy 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- OIGNJSKKLXVSLS-VWUMJDOOSA-N prednisolone Chemical compound O=C1C=C[C@]2(C)[C@H]3[C@@H](O)C[C@](C)([C@@](CC4)(O)C(=O)CO)[C@@H]4[C@@H]3CCC2=C1 OIGNJSKKLXVSLS-VWUMJDOOSA-N 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1669—Programme controls characterised by programming, planning systems for manipulators characterised by special application, e.g. multi-arm co-operation, assembly, grasping
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The invention relates to the technical field of robot control, and provides a control method and device of a mechanical arm, the mechanical arm and a readable storage medium, wherein the method comprises the steps of determining a force component in the current moving direction of the mechanical arm according to a contact force detection result fed back by a force sensor; determining a target movement speed and/or the target contact position of the mechanical arm according to the force component and the current movement direction; and controlling the mechanical arm to execute ultrasonic scanning action according to the target moving speed and/or the target contact position. The contact force between the tail end of the robot and the environment is adjusted by adjusting the speed and/or the position of the robot, and the deviation of force is corrected, so that the robot has certain flexibility in the scanning process, and the comfort level of a detector in the process of receiving ultrasonic scanning and detecting is improved.
Description
Technical Field
The present invention relates to the field of robot control technologies, and in particular, to a method and apparatus for controlling a mechanical arm, and a readable storage medium.
Background
The novel breast ultrasound scanning is usually operated by using a robot instead of ultrasonic equipment held by an ultrasonic doctor, and has the advantages of high scanning speed and labor saving.
In order to ensure that the images obtained by robot scanning are clear enough, the traditional robot scanning mode needs to perform constant force control on the robot arm, namely, the robot arm is controlled to scan a human body according to a preset track at constant speed and constant force.
However, in the breast ultrasound scanning process, the mechanical arm needs to be in direct contact with a human body, and because the mechanical arm is often rigid and constant, discomfort is easily brought to a person to be tested in the contact process with the human body, and even damage is brought to the body of the person to be tested in some serious cases. Therefore, the traditional rigid and constant force breast ultrasonic scanning mode using the mechanical arm has the problem of low comfort and safety in the scanning process.
The foregoing is provided merely for the purpose of facilitating understanding of the technical solutions of the present invention and is not intended to represent an admission that the foregoing is prior art.
Disclosure of Invention
The invention mainly aims to provide a control method of a mechanical arm, which aims to solve the problem of low comfort level of a detector in the process of receiving ultrasonic scanning detection.
In order to achieve the above object, the present invention provides a control method for a mechanical arm, the control method for a mechanical arm comprising:
determining a force component in the current moving direction of the mechanical arm according to a contact force detection result fed back by the force sensor;
Determining a target movement speed and/or the target contact position of the mechanical arm according to the force component and the current movement direction;
and controlling the mechanical arm to execute ultrasonic scanning action according to the target moving speed and/or the target contact position.
Optionally, before the step of determining the force component in the current moving direction of the mechanical arm according to the detection result of the contact force fed back by the force sensor, the method further includes:
acquiring an initial scanning track of the mechanical arm;
And controlling the mechanical arm to execute initial ultrasonic scanning action according to the initial scanning track.
Optionally, the step of determining the target movement speed and/or the target contact position of the mechanical arm according to the force component and the current movement direction comprises:
Acquiring a contact force expected value in the same direction as the force component;
determining a biasing force between the contact force expected value and the force component;
Determining a force deviation amount of the mechanical arm according to the deviation force based on an admittance transfer function;
And determining a target contact position of the mechanical arm according to the force deviation amount.
Optionally, the step of determining the force deviation amount of the mechanical arm according to the deviation force based on the admittance transfer function includes:
Acquiring inertia parameters, damping parameters and rigidity parameters corresponding to the mechanical arm;
determining an admittance transfer function associated with the mechanical arm according to the inertia parameter, the damping parameter, the rigidity parameter and the deviation force;
Determining a discrete admittance transfer function corresponding to the admittance transfer function;
And determining the force deviation amount according to the discrete admittance transfer function based on a preset sampling time.
Optionally, before the step of determining the target movement speed and/or the target contact position of the mechanical arm according to the force component and the current movement direction, the method further comprises:
Acquiring the current moving speed of the mechanical arm and acquiring the target moving speed and the target moving direction;
The step of determining the target movement speed and/or target contact position of the mechanical arm according to the force component and the current movement direction comprises:
determining an acceleration of the robotic arm based on the force component and the current movement rate;
Determining an offset rate between the current movement rate and the target movement rate, and determining an offset direction between the current movement direction and the target movement direction;
And determining the target moving speed according to the offset rate, the offset direction and the acceleration.
Optionally, the step of controlling the mechanical arm to perform the ultrasonic scanning action according to the target moving speed and/or the target contact position includes:
determining control parameters of the mechanical arm according to the target moving speed and/or determining the control parameters of the mechanical arm according to the target contact position;
and controlling the mechanical arm to execute the ultrasonic scanning action according to the control parameter.
Optionally, the control method of the mechanical arm further includes:
acquiring the current moving position of the mechanical arm;
Determining a movement amount between the current movement position and a target movement position;
and controlling the mechanical arm to move towards the target moving position according to the moving amount, wherein in the process of moving towards the target moving position, the detection result of the contact force fed back by the force sensor arranged on the tail end of the mechanical arm is kept unchanged.
In addition, in order to achieve the above object, the present invention also provides a control device for a mechanical arm, the control device for a mechanical arm including:
the admittance control module is used for determining a target contact position and/or a target moving speed of the mechanical arm according to a force detection result fed back by a force sensor at the tail end of the mechanical arm;
and the position control module is used for controlling the mechanical arm to execute ultrasonic scanning action according to the target contact position and/or the target moving speed.
In addition, in order to achieve the aim, the invention also provides a mechanical arm, which comprises a memory, a processor and a mechanical arm control program stored on the memory and capable of running on the processor, wherein the mechanical arm control program is executed by the processor to realize the steps of the mechanical arm control method.
The embodiment of the invention provides a control method and device of a mechanical arm, the mechanical arm and a computer readable storage medium, wherein the method comprises the steps of determining a force component in the current moving direction of the mechanical arm according to a contact force detection result fed back by a force sensor; determining a target movement speed and/or the target contact position of the mechanical arm according to the force component and the current movement direction; and controlling the mechanical arm to execute ultrasonic scanning action according to the target moving speed and/or the target contact position. The contact force between the tail end of the robot and the environment is adjusted by adjusting the speed and/or the position of the robot, and the deviation of force is corrected, so that the robot has certain flexibility in the scanning process, and the comfort level of a detector in the process of receiving ultrasonic scanning and detecting is improved.
Drawings
Fig. 1 is a schematic diagram of a hardware architecture of a mechanical arm according to an embodiment of the present invention;
FIG. 2 is a flowchart of a first embodiment of a method for controlling a robot arm according to the present invention;
FIG. 3 is a constant force control block diagram based on admittance control in this embodiment;
Fig. 4 is a schematic diagram of a refinement flow of step S20 in a second embodiment of the control method of the mechanical arm of the present invention;
fig. 5 is a schematic diagram of a refinement flow of step S23 in a third embodiment of the control method of the mechanical arm of the present invention;
FIG. 6 is a flowchart of a control method of a robot arm according to a fourth embodiment of the present invention;
fig. 7 is a schematic diagram of a refinement flow of step S50 in a fifth embodiment of a control method of a mechanical arm according to the present invention;
FIG. 8 is a flowchart of a control method of a robot arm according to a sixth embodiment of the present invention;
FIG. 9 is a schematic diagram of an impedance model of a control method of a robot arm according to the present invention;
Fig. 10 is a schematic structural diagram of a control device of a mechanical arm according to the present invention.
The achievement of the objects, functional features and advantages of the present invention will be further described with reference to the accompanying drawings, in conjunction with the embodiments.
Detailed Description
It should be understood that the drawings of the present invention illustrate exemplary embodiments of the invention, and that the invention may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.
As an implementation scheme, an execution device corresponding to the control method of the mechanical arm may be shown in fig. 1.
The embodiment of the invention relates to an execution device of a mechanical arm, which comprises a processor 101, such as a CPU, a memory 102 and a communication bus 103. Wherein the communication bus 103 is used to enable connected communication among the components.
The memory 102 may be a high-speed RAM memory or a stable memory (non-volatilememory), such as a disk memory. As shown in fig. 1, a control program of the robot arm may be included in a memory 102 as a kind of computer-readable storage medium, and a processor 101 may be used to call the control program of the robot arm stored in the memory 102 and perform the following operations:
acquiring an initial scanning track of the mechanical arm;
controlling the mechanical arm to execute initial ultrasonic scanning action according to the initial scanning track;
determining a force component in the current moving direction of the mechanical arm according to a contact force detection result fed back by the force sensor;
Determining a target movement speed and/or the target contact position of the mechanical arm according to the force component and the current movement direction;
and controlling the mechanical arm to execute ultrasonic scanning action according to the target moving speed and/or the target contact position.
In one embodiment, the processor 101 may be configured to call a control program of the robotic arm stored in the memory 102 and perform the following operations:
Acquiring a contact force expected value in the same direction as the force component;
determining a biasing force between the contact force expected value and the force component;
Determining a force deviation amount of the mechanical arm according to the deviation force based on an admittance transfer function;
And determining a target contact position of the mechanical arm according to the force deviation amount.
In one embodiment, the processor 101 may be configured to call a control program of the robotic arm stored in the memory 102 and perform the following operations:
Acquiring inertia parameters, damping parameters and rigidity parameters corresponding to the mechanical arm;
determining an admittance transfer function associated with the mechanical arm according to the inertia parameter, the damping parameter, the rigidity parameter and the deviation force;
Determining a discrete admittance transfer function corresponding to the admittance transfer function;
And determining the force deviation amount according to the discrete admittance transfer function based on a preset sampling time.
In one embodiment, the processor 101 may be configured to call a control program of the robotic arm stored in the memory 102 and perform the following operations:
Acquiring the current moving speed of the mechanical arm and acquiring the target moving speed and the target moving direction;
The step of determining the target movement speed and/or target contact position of the mechanical arm according to the force component and the current movement direction comprises:
determining an acceleration of the robotic arm based on the force component and the current movement rate;
Determining an offset rate between the current movement rate and the target movement rate, and determining an offset direction between the current movement direction and the target movement direction;
And determining the target moving speed according to the offset rate, the offset direction and the acceleration.
In one embodiment, the processor 101 may be configured to call a control program of the robotic arm stored in the memory 102 and perform the following operations:
determining control parameters of the mechanical arm according to the target moving speed and/or determining the control parameters of the mechanical arm according to the target contact position;
and controlling the mechanical arm to execute the ultrasonic scanning action according to the control parameter.
In one embodiment, the processor 101 may be configured to call a control program of the robotic arm stored in the memory 102 and perform the following operations:
acquiring the current moving position of the mechanical arm;
Determining a movement amount between the current movement position and a target movement position;
and controlling the mechanical arm to move towards the target moving position according to the moving amount, wherein in the process of moving towards the target moving position, the detection result of the contact force fed back by the force sensor arranged on the tail end of the mechanical arm is kept unchanged.
In traditional breast ultrasound examination, a doctor is required to hold an ultrasonic device for manual operation, the scanning speed is low, and a great deal of experience of the sonographer is required. The novel breast ultrasound scanning mode adopts a robot to replace the operation of an ultrasonic doctor, has the advantages of high scanning speed and labor saving, but only uses a position mode to control the robot to scan, the obtained image is often fuzzy, the quality is low, and the film reading and judgment of the ultrasonic doctor are seriously affected. The feedback of the experienced ultrasonic doctor and a large number of experiments prove that the excellent ultrasonic image needs the robot to perform constant force control, but the existing constant force control method directly used in mammary gland scanning has the defects of unsatisfactory effect, larger steady state error and slower tracking force speed. The breast ultrasonic scanning scene is special, the robot directly acts on the human body, the mechanical arm with pure position control has high rigidity, the smaller position error can generate high pressure, and the pressure directly acts on the breast of the human body can lead to breast deformation and even threaten the life safety of females. In such a scenario, the control effect can be improved by adopting admittance control.
Based on the hardware architecture of the control device of the mechanical arm based on the robot control technology, the embodiment of the control method of the mechanical arm is provided.
Referring to fig. 2, in a first embodiment, the control method of the mechanical arm includes the steps of:
Step S10, acquiring an initial scanning track of the mechanical arm;
step S20, controlling the mechanical arm to execute initial ultrasonic scanning action according to the initial scanning track;
In the embodiment, the robot controls the mechanical arm to execute ultrasonic scanning action on the detected human body based on a position mode, in the position mode, the robot determines an initial scanning track of the mechanical arm in the ultrasonic scanning process according to preset scanning logic, the tail end of the mechanical arm is in contact with the detected human body, moves according to the initial scanning track, and executes the scanning action.
Step S30, determining a force component in the current moving direction of the mechanical arm according to a contact force detection result fed back by the force sensor;
In the embodiment, admittance control is impedance control based on a position mode, a force sensor is arranged at the tail end of the robot, the tail end contact force is measured, flexible control is realized, the situation that brute force directly acts on a human body is avoided, and constant force control is realized. The tail end of the mechanical arm is provided with a force sensor in communication connection with the mechanical arm control device, the force sensor can be a multidimensional force sensor (such as a six-dimensional force sensor), in the process of ultrasonic scanning of the mechanical arm, the force component of the output force of the mechanical arm in the moving direction according to the ultrasonic scanning track is determined according to the contact force detection result fed back in real time on the multidimensional force sensor, the acting force of the contact part of the mechanical arm and a human body can be determined in real time according to the force component, and the constant force control of the mechanical arm in the process of ultrasonic scanning is maintained.
Step S40, determining a target moving speed and/or the target contact position of the mechanical arm according to the force component and the current moving direction;
and S50, controlling the mechanical arm to execute ultrasonic scanning action according to the target moving speed and/or the target contact position.
In this embodiment, based on the admittance control principle, the target movement position of the mechanical arm can be obtained according to the force component and the current movement direction of the mechanical arm, and when the robot is acted by an external force, the robot will deviate on the original track to conform to the external force, the implementation method is that a new expected position Δx is generated at the admittance controller, and the control target movement from X to Δx is implemented under the action of the internal position closed-loop controller.
Referring to fig. 3, fig. 3 is a constant force control block diagram based on admittance control in the present embodiment. Wherein F 0 is a force expected value, namely a target value to be tracked, deltaX= [ Deltax, deltay, deltaalpha, deltabeta, deltagamma ], represents the offset of the robot in each direction, deltaF= [ DeltaF X,ΔFY,ΔFz,ΔTx,ΔTy,ΔTz ] represents the deviation signal of the contact force of the robot in each direction. The expected value of the force F 0 is a debugged pressure value (15-20N) which can prevent a detector from feeling uncomfortable in the ultrasonic scanning process of the robot, delta F is acquired based on a force sensor, the deviation is carried out according to the deviation delta X of a control mechanical arm by a position controller according to the admittance control principle, the contact force between the contact part of the mechanical arm and the detector after the deviation is changed to an expected value F 0, the value on the force sensor is correspondingly changed to F 0, and therefore the purposes of measuring the contact between the robot and the environment, designing the controller according to the deviation between the contact force and the ideal value, adjusting the contact force between the tail end of the robot and the environment and correcting the force deviation are achieved by adjusting the speed or the position of the robot.
In some embodiments, in the ultrasonic scanning process of the robot, contact force data fed back by a force sensor at the tail end of the mechanical arm is detected in real time, the contact force data is matched with a preset force expected value according to a certain frequency, so that the contact force is ensured to be the same as the force expected value as much as possible in the ultrasonic scanning process, and the contact force is indirectly changed through admittance control in the system, so that the robot has a certain compliance capability in the scanning process.
According to the technical scheme provided by the embodiment, the force sensor is arranged at the tail end of the mechanical arm, the target moving speed and/or the target contact position of the mechanical arm are determined according to the component force which is fed back by the force sensor and is the same as the moving direction of the mechanical arm, and the mechanical arm is controlled to execute ultrasonic scanning action according to the target moving speed and/or the target contact position, so that the robot has certain flexibility in the scanning process, and the comfort of a detector in the process of receiving ultrasonic scanning detection is improved.
Referring to fig. 4, in the second embodiment, based on the first embodiment, the step S20 includes:
step S21, acquiring a contact force expected value in the same direction as the force component;
Step S22 of determining a deviation force between the contact force expected value and the force component;
step S23, determining the force deviation amount of the mechanical arm according to the deviation force based on an admittance transfer function;
and step S24, determining the target contact position of the mechanical arm according to the force deviation amount.
Alternatively, the present embodiment provides a way to control the tip displacement when an external force is sensed by the force sensor. In the ultrasonic scanning process, as the detection part for detecting the human body is not planar and has a certain three-dimensional structure, when the mechanical arm moves according to the ultrasonic scanning track, the contact position of the mechanical arm and the contact part needs to be changed according to the distance between the corresponding contact part and the moment of the mechanical arm so as to avoid uncomfortable feeling and damage caused by overlarge pressure on the body in the ultrasonic scanning process.
In some embodiments, when the robot arm scans the chest (e.g., from the abdomen to the abdomen of the human body) while the test person lies on the test table, the chest is relatively close to the moment of the robot arm, so if the original moment is maintained to scan the chest, the pressure between the end of the robot arm and the contact part is increased, which causes discomfort to the test person. In order to avoid overlarge pressure, acquiring force parameters of the tail end of the mechanical arm in real time in the scanning process, matching the force parameters with expected contact force, and changing the moment of the mechanical arm according to the deviation through an admittance transfer function when deviation occurs between the force parameters and the expected contact force, so as to change the target contact position of the mechanical arm according to the moment. Therefore, the contact force of the tail end of the mechanical arm is always consistent with an expected value, and discomfort and damage caused by overlarge pressure on the body of a detected human body in the ultrasonic scanning process are avoided.
In the technical scheme provided by the embodiment, the mode of changing the target contact position of the mechanical arm by changing the moment of the mechanical arm ensures that the contact force of the tail end of the mechanical arm is consistent with the expected value all the time, and avoids discomfort and even damage caused by overlarge pressure on the body of a detected human body in the ultrasonic scanning process.
Referring to fig. 5, in the third embodiment, based on the second embodiment, the step S23 includes:
Step S231, acquiring inertia parameters, damping parameters and rigidity parameters corresponding to the mechanical arm;
step S232, determining an admittance transfer function associated with the mechanical arm according to the inertia parameter, the damping parameter, the rigidity parameter and the deviation force;
Step S233, determining a discrete admittance transfer function corresponding to the admittance transfer function;
Step S234, determining the force deviation amount according to the discrete admittance transfer function based on a preset sampling time.
Optionally, admittance control is a control algorithm of the invention, and the admittance control is to set up the relation between the displacement of the tail end of the robot and the contact force by making the tail end force position control of the robot equivalent to a spring-mass-damping model, and to realize the adjustment force by randomly adjusting damping and stiffness parameters, thereby the relation between the tail end position of the robot and the contact force. The impedance model is typically set as a second order differential equation as follows:
Wherein M d is an inertial parameter, B d is a damping parameter, K d is a rigidity parameter, and X r -X) is a terminal displacement;
without considering M d, the impedance model is simplified as:
after the above formula is converted by pulling, the following steps are obtained:
H is a system admittance transfer function, B d and K d are admittance parameters, and admittance control is realized by changing the two parameters;
Discretizing the transfer function by adopting a first-order backward difference:
Order the Wherein T is the sampling time;
substituting the transfer function to obtain:
further derivation, yield:
Finally, the method comprises the following steps:
Admittance control of the mechanical arm can be completed by adjusting k 1 and k 2.
In the technical scheme provided by the embodiment, the admittance transfer function of the mechanical arm is determined through the inertia parameter, the damping parameter and the rigidity parameter corresponding to the mechanical arm and the deviation force, and the damping and rigidity parameters of the mechanical arm are adjusted based on a model constructed by the admittance transfer function so as to adjust the force, so that the flexible control of the mechanical arm is realized.
Referring to fig. 6, in the fourth embodiment, before step S20, based on any embodiment, the method further includes:
Step S60, acquiring the current moving speed of the mechanical arm, and acquiring a target moving speed and a target moving direction;
The step S20 includes:
step S25, determining the acceleration of the mechanical arm according to the force component and the current moving speed;
Step S26 of determining an offset rate between the current movement rate and the target movement rate, and determining an offset direction between the current movement direction and the target movement direction;
And step S27, determining the target moving speed according to the offset speed, the offset direction and the acceleration.
Alternatively, the present embodiment provides a way to control the speed of movement of the tip when the force sensor senses an external force. In the scanning process of the mechanical arm, the mechanical arm can conduct track scanning at a certain initial speed, and the initial speed comprises an initial speed and an initial direction. Because the mechanical arm needs to be in real-time contact with a human body in the scanning process, the requirements of different scanning positions on the moving speed and the moving direction of the mechanical arm are different, in the embodiment, the moving speed and the moving direction corresponding to different scanning positions are fixed preset values, namely, the expected moving speed and the expected moving direction, when the mechanical arm needs to change in the scanning process according to the scanning track, the acceleration a is determined through the force component F and the current moving speed of the mechanical arm, the speed difference value between the target moving speed and the current moving speed and the direction angle difference value between the target moving direction and the current offset direction are determined, and the target moving speed after the mechanical arm is changed can be determined based on the offset speed, the offset direction and the acceleration, wherein the target moving speed comprises the target moving speed and the target moving direction of the mechanical arm, so that the mechanical arm is controlled to change the speed and the direction according to the scanning track.
In the technical scheme provided by the implementation, the flexible control of the robot is realized through the matching of the guiding force and the speed and the acceleration of the tail end of the mechanical arm.
Referring to fig. 7, in a fifth embodiment, based on any one of the embodiments, the step S50 includes:
Step S51, determining control parameters of the mechanical arm according to the target moving speed and/or determining the control parameters of the mechanical arm according to the target contact position;
and step S52, controlling the mechanical arm to execute the ultrasonic scanning action according to the control parameters.
Optionally, since the control of the mechanical arm based on the speed/displacement is two different control methods, the obtained control parameters are different, and the determination method of the target moving speed and the target contact position has been given in the foregoing, in this embodiment, if the mechanical arm is controlled based on the moving speed, the acceleration parameter of the distal end of the mechanical arm and the target moving speed parameter are determined according to the feedback force data of the force sensor to perform ultrasonic scanning, and if the mechanical arm is controlled based on the displacement, the force parameter of the mechanical arm is determined according to the feedback force data to determine the target contact position of the mechanical arm. It is emphasized that admittance control is performed on the basis of speed or displacement, so that the contact force between the mechanical arm and the target body part in the scanning process is consistent with the expected contact force, and discomfort and damage caused by excessive pressure on the body in the ultrasonic scanning process of a detected human body are avoided.
Alternatively, the robotic arm may also control the robotic arm based on both velocity and displacement to achieve more precise robotic arm control.
In the technical scheme provided by the embodiment, the control parameters of the mechanical arm are determined through the target moving speed and/or the target contact position, so that the contact force between the mechanical arm and the target body part in the scanning process is consistent with the expected contact force, and discomfort and damage caused by excessive pressure on the body in the ultrasonic scanning process of a detected human body are avoided.
Referring to fig. 8, in a sixth embodiment, based on any one of the embodiments, the control method of the mechanical arm further includes:
step S70, acquiring the current moving position of the mechanical arm;
step S80, determining the movement amount between the current movement position and the target movement position;
and step S90, controlling the mechanical arm to move towards the target position according to the movement amount.
Alternatively, the control method of the mechanical arm may 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 the impedance force according to the position deviation amount generated by the end of the robot, and the impedance control method is a control method based on the dynamic relation between the force and the speed or the position. The dynamic relation between the robot motion and the external acting force is established, and the robot motion is regulated, so that the dynamic regulation of the robot and the environment is realized.
Referring to fig. 9, fig. 9 is a schematic diagram of an impedance model, and the dynamics equation of the impedance control method is:
Wherein X is displacement, M d is mass, B d is damping, K d is rigidity, and F is acting force;
The Laplace transform is performed on the above dynamics equation, so that the following can be obtained:
(MdS2+BdS+Kd)X(s)=F(s)
Since impedance is defined as Z(s) =f (s)/X(s), admittance is defined as the inverse of impedance:
Y(s)=Z-1(s)=X(s)/F(s)
since Δx=yΔf, the displacement change Δx can be generated by the change Δf in force.
In addition, referring to fig. 10, the present embodiment further proposes a control device for a mechanical arm, where the control device for a mechanical arm includes:
The admittance control module 100 is configured to determine a target contact position and/or a target movement speed of the mechanical arm according to a force detection result fed back by a force sensor at the end of the mechanical arm;
and the position control module 200 is used for controlling the mechanical arm to execute ultrasonic scanning action according to the target contact position and/or the target moving speed.
In addition, the invention also provides a mechanical arm, which comprises a memory, a processor and a mechanical arm control program stored on the memory and capable of running on the processor, wherein the mechanical arm control program realizes the steps of the mechanical arm control method according to any one of the above steps when being executed by the processor.
In addition, the present invention also provides a computer readable storage medium storing a control program of the mechanical arm, where the control program of the mechanical arm implements each step of the control method of the mechanical arm described in the above embodiment when executed by a processor.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
From the above description of the embodiments, it will be clear to those skilled in the art that the above-described embodiment method may be implemented by means of software plus a necessary general hardware platform, but of course may also be implemented by means of hardware, but in many cases the former is a preferred embodiment. Based on such understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art in the form of a software product stored in a computer readable storage medium (e.g. ROM/RAM, magnetic disk, optical disk) as described above, comprising instructions for causing a terminal device (which may be a mobile phone, a computer, a server, or a network device, etc.) to perform the method according to the embodiments of the present invention.
The foregoing description is only of the preferred embodiments of the present invention, and is not intended to limit the scope of the invention, but rather is intended to cover any equivalents of the structures or equivalent processes disclosed herein or in the alternative, which may be employed directly or indirectly in other related arts.
Claims (10)
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210702089.0A CN115179279B (en) | 2022-06-21 | 2022-06-21 | Control method and device of mechanical arm, mechanical arm and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210702089.0A CN115179279B (en) | 2022-06-21 | 2022-06-21 | Control method and device of mechanical arm, mechanical arm and readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115179279A CN115179279A (en) | 2022-10-14 |
| CN115179279B true CN115179279B (en) | 2025-02-18 |
Family
ID=83514662
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210702089.0A Active CN115179279B (en) | 2022-06-21 | 2022-06-21 | Control method and device of mechanical arm, mechanical arm and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115179279B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115946120B (en) * | 2023-01-09 | 2024-02-13 | 上海艾利特机器人有限公司 | Mechanical arm control method, device, equipment and medium |
| CN115990883B (en) * | 2023-01-12 | 2024-04-26 | 之江实验室 | Robot control method and device |
| CN115781695B (en) * | 2023-01-30 | 2023-06-06 | 戴盟(深圳)机器人科技有限公司 | Touch sensor module, device and mobile operation robot control method |
| CN116175584B (en) * | 2023-03-09 | 2025-07-15 | 中国科学院深圳先进技术研究院 | Ultrasonic robot scanning control method, device, equipment and storage medium |
| CN116175594B (en) * | 2023-04-26 | 2023-06-30 | 北京宸普豪新科技有限公司 | Robot arm control method and equipment and robot |
| WO2025007230A1 (en) * | 2023-07-03 | 2025-01-09 | 深圳华大智造云影医疗科技有限公司 | Robot position processing method and apparatus, electronic device, and storage medium |
| CN118383801B (en) * | 2024-06-27 | 2024-09-06 | 合肥合滨智能机器人有限公司 | Full-automatic breast ultrasound double-arm scanning method, device and control system |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111481231A (en) * | 2019-01-29 | 2020-08-04 | 昆山华大智造云影医疗科技有限公司 | Ultrasonic detection control method and device and computer readable storage medium |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB0121984D0 (en) * | 2001-09-11 | 2001-10-31 | Isis Innovation | Method and apparatus for ultrasound examination |
| DE102017209034A1 (en) * | 2017-05-30 | 2018-12-06 | Kuka Deutschland Gmbh | MANIPULATOR SYSTEM WITH INGREDIENT FOR POWER REDUCTION |
| CN109009211B (en) * | 2018-06-22 | 2023-07-21 | 联想(北京)有限公司 | Intelligent equipment, ultrasonic detection-based method and device |
| WO2020154921A1 (en) * | 2019-01-29 | 2020-08-06 | 昆山华大智造云影医疗科技有限公司 | Ultrasound scanning control method and system, ultrasound scanning device, and storage medium |
| CN112223283B (en) * | 2020-09-29 | 2024-06-25 | 腾讯科技（深圳）有限公司 | Mechanical arm, robot, control method of mechanical arm, processing equipment and medium |
| CN112716522A (en) * | 2020-12-30 | 2021-04-30 | 无锡祥生医疗科技股份有限公司 | Probe tail end trajectory tracking method and device, electronic equipment and storage medium |
-
2022
- 2022-06-21 CN CN202210702089.0A patent/CN115179279B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111481231A (en) * | 2019-01-29 | 2020-08-04 | 昆山华大智造云影医疗科技有限公司 | Ultrasonic detection control method and device and computer readable storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115179279A (en) | 2022-10-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115179279B (en) | Control method and device of mechanical arm, mechanical arm and readable storage medium | |
| US10668625B2 (en) | Robot arm apparatus, robot arm apparatus control method, and program | |
| CN111660306B (en) | A robot variable admittance control method and system based on operator comfort | |
| JP4319232B2 (en) | Power assist device and control method thereof | |
| JP5181957B2 (en) | Robot control apparatus, robot control method, and robot control program | |
| KR101964332B1 (en) | Method of hand-eye calibration, computer program for executing the method, and robot system. | |
| CN109249394B (en) | Robot control method and system based on admittance control algorithm | |
| US10365630B2 (en) | Control method of profile measuring apparatus | |
| JP6859557B2 (en) | Treatment device | |
| CN115756007A (en) | Variable admittance control system and method for coupling interaction of human, environment and robot | |
| WO2024183371A1 (en) | Ultrasonic robot scanning control method and apparatus, device, and storage medium | |
| JP5428639B2 (en) | Robot control apparatus and robot teaching method | |
| US12307703B2 (en) | Detection device and detection method | |
| CN116852397B (en) | Self-adaptive adjusting method for physiotherapy force and physiotherapy path of negative pressure physiotherapy robot | |
| JP5027633B2 (en) | Ultrasonic transducer | |
| CN107748496A (en) | Impedance controller algorithm based on parameter adaptive regulation | |
| CN114795289A (en) | Robot scanning method and device for visual servo of blood vessel ultrasonic image | |
| CN115363682B (en) | Saw blade bending measurement method, surgical robot correction method and surgical robot | |
| CN116236227A (en) | A robot ultrasonic probe control method, system, device and storage medium | |
| CN119606418A (en) | Robotic arm ultrasonic scanning method, system and storage medium based on contact force control | |
| CN116236227B (en) | Robot ultrasonic probe control method, system, equipment and storage medium | |
| JP2021074788A (en) | Robot system | |
| CN119564259A (en) | Robot-assisted B-ultrasound detection system and control method | |
| US20230371930A1 (en) | Ultrasonic diagnostic apparatus | |
| CN118750023B (en) | Robotic ultrasonic elastic imaging method and system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |