CN117944056A

CN117944056A - Six-dimensional force sensor-based mechanical arm control method and device

Info

Publication number: CN117944056A
Application number: CN202410349092.8A
Authority: CN
Inventors: 江维; 江国豪; 蔺志伟
Original assignee: Beijing Yunli Jingan Technology Co ltd
Current assignee: Beijing Yunli Jingan Technology Co ltd
Priority date: 2024-03-26
Filing date: 2024-03-26
Publication date: 2024-04-30

Abstract

The application discloses a mechanical arm control method and device based on a six-dimensional force sensor. The operating device is stressed by the tension or compression force of the soft endoscope. If the operating device is stressed as a tensile force and the operating device is stressed to reach the tension threshold of the soft endoscope, the tensile force of the soft endoscope is larger. In order to avoid damage to the soft endoscope due to high tension, the track point position of the operation arm can be adjusted to a direction approaching to the conveying device on the basis of the next preselected track point to obtain a first track point. The operating arm is controlled according to the first track point. Therefore, the track point of the operation arm is adjusted based on the stress condition of the soft endoscope, so that the soft endoscope is prevented from being damaged, and the accurate regulation and control of the following position of the operation arm can be realized.

Description

Six-dimensional force sensor-based mechanical arm control method and device

Technical Field

The application relates to the technical field of mechanical arm control, in particular to a mechanical arm control method and device based on a six-dimensional force sensor.

Background

The natural cavity tracts such as the digestive tract, the respiratory tract and the like are good sites for common diseases of human beings. Since the lesion is located inside the natural lumen of the human body, it is necessary to perform an examination by a soft endoscope.

Traditional soft endoscopy or operation requires the actions of a person to match with the holding mirror body, the operation knob, the manual conveying mirror body and the like by both hands to finish the operation process. Moreover, some examinations or operations are conducted under the image guidance of radiation, and medical staff is required to manually operate the soft endoscope in the case of wearing heavy lead protective clothing for a long period of time. The medical staff diagnosis and treatment operation quality, physical strength and health are greatly influenced.

With the development of robot auxiliary technology, medical staff can send the mirror and operate under the mirror through operating the robot, which can greatly reduce the physical strength and manual operation fatigue of the medical staff and reduce the radiation to the medical staff. At present, a soft endoscope control robot system needs to conduct operation arm track planning in advance according to the position of a natural cavity opening of a patient in the process of delivering the endoscope so as to control an operation arm according to the operation arm track. However, the trajectory of the operating arm used by the healthcare worker may not be accurate, so that the operating arm cannot be precisely controlled.

Disclosure of Invention

In order to solve the technical problems, the application provides a six-dimensional force sensor-based mechanical arm control method and a six-dimensional force sensor-based mechanical arm control device, which can accurately regulate and control the following position of an operation arm on the basis of the track of the currently used operation arm.

In order to achieve the above purpose, the technical scheme provided by the application is as follows:

in a first aspect, the present application provides a six-dimensional force sensor-based mechanical arm control method, which is applied to a soft endoscope operation robot system, wherein the soft endoscope operation robot system comprises a conveying arm, an operation arm and a first six-dimensional force sensor; the first six-dimensional force sensor is arranged on an operation device at the tail end of the operation arm; the method comprises the following steps:

acquiring a next preselected track point of the operation arm; the next preselected track point belongs to a track point in a preset track of the operation arm;

acquiring three-dimensional orthogonal force measured by the first six-dimensional force sensor, and calculating the stress of an operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor; the operating device is stressed by the tension or compression force of the soft endoscope;

When the stress of the operation device is a tensile force and the stress of the operation device reaches a tension threshold of the soft endoscope, updating the next preselected track point to a first track point, and controlling the operation arm according to the first track point; in the preset track, a first number of track points are spaced between the first track point and the next preselected track point, and the distance between the first track point and the conveying device at the tail end of the conveying arm is smaller than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

In a second aspect, the present application provides a six-dimensional force sensor-based mechanical arm control device, which is applied to a soft endoscope operation robot system, wherein the soft endoscope operation robot system comprises a conveying arm, an operation arm and a first six-dimensional force sensor; the first six-dimensional force sensor is arranged on an operation device at the tail end of the operation arm; the device comprises:

a first acquisition unit for acquiring a next preselected trajectory point of the operation arm; the next preselected track point belongs to a track point in a preset track of the operation arm;

A second acquisition unit for acquiring the three-dimensional orthogonal force measured by the first six-dimensional force sensor, and calculating the stress of the operation device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor; the operating device is stressed by the tension or compression force of the soft endoscope;

The updating unit is used for updating the next preselected track point into a first track point when the stress of the operating device is a tensile force and the stress of the operating device reaches a tension threshold of the soft endoscope, and controlling the operating arm according to the first track point; in the preset track, a first number of track points are spaced between the first track point and the next preselected track point, and the distance between the first track point and the conveying device at the tail end of the conveying arm is smaller than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

In a third aspect, the present application provides an electronic device comprising:

one or more processors;

a storage device having one or more programs stored thereon,

The one or more programs, when executed by the one or more processors, cause the one or more processors to implement the six-dimensional force sensor based robotic arm control method as described in the first aspect.

In a fourth aspect, the present application provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the six-dimensional force sensor based robotic arm control method according to the first aspect.

According to the technical scheme, the application has the following beneficial effects:

The application provides a mechanical arm control method and device based on a six-dimensional force sensor. The first six-dimensional force sensor is mounted on the operating device at the end of the operating arm. And acquiring the next preselected track point of the operation arm, wherein the next preselected track point belongs to the track point in the preset track of the operation arm. The next preselected trajectory point determined may be an inaccurate trajectory point. Based on this, the three-dimensional orthogonal force currently measured by the first six-dimensional force sensor is obtained. And calculating the stress of the operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor, wherein the stress of the operating device is the tension or compression force of the soft endoscope. And if the calculated current operating device stress is a tensile force and the operating device stress reaches the tension threshold of the soft endoscope, updating the next preselected track point to be a first track point, and controlling the operating arm according to the first track point. In the preset track, a first track point and a next preselected track point are separated by a first number of track points, and the distance between the first track point and the conveying device at the tail end of the conveying arm is smaller than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

It can be seen that after the next preselected locus is determined, if the current operating device is under tension and the operating device is under tension, this means that the soft endoscope is under tension and the force of the soft endoscope being under tension exceeds the soft endoscope tension threshold. That is, the soft endoscope receives a large tensile force. In order to avoid damage to the soft endoscope due to high tension, the track point position of the operation arm can be adjusted to a direction approaching to the conveying device on the basis of the next preselected track point to obtain a first track point. Further, the operation arm is controlled according to the first locus point. Therefore, in order to avoid the damage of the soft endoscope as much as possible, the track point of the operation arm is adjusted based on the actual stress condition of the soft endoscope, so that the accurate regulation and control of the following position of the operation arm are realized, and the quick conveying of the soft endoscope can be facilitated.

Drawings

In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, and it is obvious that the drawings in the following description are some embodiments of the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.

FIG. 1a is a schematic view of a conventional soft endoscope according to an embodiment of the present application;

FIG. 1b is a schematic diagram of a soft endoscope operation robot system according to an embodiment of the present application;

Fig. 2 is a flowchart of a method for controlling a mechanical arm based on a six-dimensional force sensor according to an embodiment of the present application;

FIG. 3 is a schematic diagram illustrating the installation of a first six-dimensional force sensor according to an embodiment of the present application;

Fig. 4 is a schematic diagram of a control method of a mechanical arm based on a six-dimensional force sensor according to an embodiment of the present application;

FIG. 5 is a schematic diagram illustrating the installation of a second six-dimensional force sensor according to an embodiment of the present application;

Fig. 6 is a schematic structural diagram of a mechanical arm control device based on a six-dimensional force sensor according to an embodiment of the present application.

Detailed Description

In order that the above-recited objects, features and advantages of the present application will become more readily apparent, a more particular description of embodiments of the application will be rendered by reference to the appended drawings and appended drawings.

It will be appreciated that the drawbacks associated with current operating arm control schemes are all the results of applicant's practices and careful study. Accordingly, the discovery process of the problems and the solutions presented hereinafter by embodiments of the present application to the problems described above should be all contributions of the applicant to the embodiments of the present application in the process of the present application.

In order to facilitate understanding of the six-dimensional force sensor-based mechanical arm control method provided by the embodiment of the present application, a related description of the soft endoscope is first made with reference to fig. 1a and 1 b. Fig. 1a is a schematic diagram of a conventional soft endoscope according to an embodiment of the present application, and fig. 1b is a schematic diagram of a soft endoscope operation robot system according to an embodiment of the present application.

In the embodiment of the present application, taking the soft endoscope as an example of the digestion soft endoscope, as shown in fig. 1a, the whole structure of the soft endoscope includes an operation portion 11, an insertion portion 12 and a head end portion 13. The operation unit 11 includes an endoscope station interface 110, a large pulsator 111, a small pulsator 112, a function button 113, a suction valve button 114, a water vapor valve button 115, and an instrument channel 116. When the soft endoscope is operated, the head end portion 13 is first controlled to be inserted into the oral cavity of the patient, and then the insertion portion 12 is controlled to be inserted into the oral cavity of the patient.

Referring to fig. 1a, taking a digestion soft endoscope as an example, a traditional manual operation digestion soft endoscope is specifically that a medical staff holds the front end of the soft endoscope with the right hand, places the left hand-held soft endoscope operation part in front of the chest, adjusts the size knob with the thumb, the middle finger and the ring finger, controls the air injection and the water injection, and controls the suction valve button with the index finger. When controlling the forward and backward movement of the soft endoscope, the endoscope body needs to be rotated, and a good visual field is always maintained. Reaching the focus or needing biopsy, the corresponding instrument passes through the instrument channel inside the insertion part from the instrument channel to the head end. And the corresponding operation is completed under the cooperation of the visual field under the mirror.

Referring to fig. 1b, the soft endoscope operation robot system may generally include a robot cart 100, a doctor-side console 200 (for manipulating the cart), a work station 300, an operation table 400, a transfer arm (may also be referred to as a master arm) 101, an operation arm (may also be referred to as a slave arm) 102, a transfer device (may also be referred to as a transfer section) 103 at the end of the transfer arm, and an operation device (may also be referred to as an operation section) 104 at the end of the operation arm. The transport arm 101 and the operation arm 102 are collectively referred to as a robot arm. Wherein, the conveying arm 101 and the operation arm 102 are all multi-degree-of-freedom mechanical arms. In soft endoscopic robotic systems, it is often necessary to secure a tool at the end of a robotic arm. For example, an operation device 104 is fixed to the distal end of the operation arm, an operation unit 11 for a soft endoscope is fixed to the operation device 104, and a transport device 103 is fixed to the distal end of the transport arm 101. The handling device 104 and the handling part 11 of the soft endoscope can be regarded jointly as a handling arm end-securing tool, and the delivery device can be regarded as a delivery arm end-securing tool, which can also be referred to as an end effector.

In the master-slave control mode, the medical staff member can generate operation commands at the doctor-side console 200, which can be converted into movements of the transport arm 101, the operation arm 102, the transport device 103 and the operation device 104. Wherein, the operation device 104 operates the bending and bending of the soft endoscope, the conveying device 103 controls the length of the soft endoscope entering the human body, and the two devices simultaneously control the rotation of the soft endoscope, and the other device can also be independently controlled and the other device can follow. At the same time, the manipulator 104 can drive the instrument associated with the soft endoscope.

According to the embodiment of the application, on the basis of the preset track (composed of a plurality of track points) of the existing operation arm, the following position of the operation arm can be accurately regulated and controlled based on the six-dimensional force sensor. The six-dimensional force sensor comprises a first six-dimensional force sensor, and the first six-dimensional force sensor is arranged on an operation device at the tail end of the operation arm. In specific implementation, the next preselected track point of the operation arm is obtained firstly based on the preset track of the operation arm. But the next preselected trajectory point determined may be an inaccurate trajectory point. Based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor currently, the stress of the operating device is calculated based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor, and the stress of the operating device is the tension or compression force of the soft endoscope. If the calculated current operating device stress is a tensile force and the operating device stress reaches the soft endoscope tensile force threshold, the soft endoscope stress is a tensile force and the soft endoscope tensile force exceeds the soft endoscope tensile force threshold. That is, the soft endoscope receives a large tensile force. At this time, the next preselected track point is updated to the first track point. The first trajectory point is a trajectory point closer to the conveyor in the preset trajectory than the next preselected trajectory point. Further, by controlling the operation arm according to the first locus point, the situation that the tension of the soft endoscope is large can be relieved. Therefore, on the basis of the original preset track, the accurate regulation and control of the following position of the operation arm is realized, and the soft endoscope conveying can be completed quickly.

In order to facilitate understanding of the present application, a method for controlling a mechanical arm based on a six-dimensional force sensor according to an embodiment of the present application is described below with reference to the accompanying drawings.

Referring to fig. 2, the flowchart of a method for controlling a mechanical arm based on a six-dimensional force sensor according to an embodiment of the present application may be applied to an upper computer of a console in a soft endoscope operation robot system, which is not limited herein. As shown in fig. 2, the method may include S201-S203:

s201: acquiring the next preselected track point of the operation arm; the next preselected trajectory point belongs to a trajectory point in the preset trajectory of the operating arm.

Before the operation of the operation arm, a preset track of the operation arm needs to be acquired. Further, the operation arm may be controlled according to a preset track when the operation arm is controlled to run later. The preset trajectory is typically composed of a plurality of trajectory points, which may be pre-calibrated or otherwise obtained by a healthcare worker, without limitation.

For example, after the preset trajectory is pre-calibrated or otherwise acquired by a healthcare worker, each of the trajectory points in the preset trajectory may be matched by the delivery length of the soft endoscope. For example, after the current delivery length of the soft endoscope is detected, a track point corresponding to the current delivery length can be obtained from a preset track, and the track point is the next preselected track point for controlling the operation of the operation arm. Wherein, can install the transport displacement increment sensor in the conveyor of the transport arm, this transport displacement increment sensor can detect the transport displacement of soft endoscope. The conveying displacement of the soft endoscope is the conveying length of the soft endoscope.

In the embodiment of the application, the track points of the operation arm are described by the pose of the operation arm, and the pose of the operation arm includes the spatial position of the operation arm (specifically, the position of the center point of the tool with the end of the operation device fixed) and the pose of the operation arm. The gesture of the operation arm comprises a pitch angle, a deflection angle, a rotation angle and the like of the operation arm. Therefore, after determining the next preselected locus point of the operation arm, the next spatial position of the operation arm and the gesture of the operation arm at the spatial position can be determined, and the operation of the operation arm can be controlled based on the next spatial position.

S202: acquiring three-dimensional orthogonal force measured by a first six-dimensional force sensor, and calculating the stress of an operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor; the operating device is stressed by the tension or compression force of the soft endoscope.

It will be appreciated that the next preselected trajectory point obtained from the preset trajectory may not be accurate, and therefore the trajectory point may also need to be adjusted according to the actual situation of the soft endoscope handling robot system (e.g. stress situation of the soft endoscope, delivery state, etc.).

In embodiments of the present application, adjustment of the tracking points may be achieved based on six-dimensional force sensors. Specifically, the soft endoscope operation robot system further includes a first six-dimensional force sensor (which may also be referred to as an operation portion six-dimensional force sensor) mounted on the operation device at the end of the operation arm. Referring to fig. 3, fig. 3 is a schematic installation diagram of a first six-dimensional force sensor according to an embodiment of the present application. As shown in fig. 3, the first six-dimensional force sensor is attached to the manipulator at the distal end of the manipulator arm, in addition to the manipulator portion of the flexible endoscope.

The coordinate system of the six-dimensional force sensor may be denoted as S. As shown in fig. 3, the first six-dimensional force sensor coordinate system includes an x-axis, a y-axis, and a z-axis. The direction of the z axis is the direction pointing to the soft endoscope operation part, the x axis direction is outward, and the y axis direction is mutually perpendicular to the z axis direction and the x axis direction. In addition, since the operation device is rigidly connected to the first six-dimensional force sensor, the x-axis direction, the y-axis direction, and the z-axis direction in the operation device coordinate system are the same as the directions of the corresponding axes in the first six-dimensional force sensor coordinate system. It can be seen that the first six-dimensional force sensor coordinate system, the operating device coordinate system, and the like are all Cartesian coordinate systems, and conform to the right-hand rule.

During the delivery of the soft endoscope, the force of the soft endoscope which is pulled or pressed is transmitted to the operation device, and the force of the operation device can be equal to the force of the soft endoscope which is pulled or pressed. The force that a soft endoscope is pulled or pressed may also be referred to as an endoscopic force. The first six-dimensional force sensor is mounted on the operating device at the end of the operating arm, and the force obtained based on the first six-dimensional force sensor is applied to the operating device. The operating device force indicates an operating device force state (i.e., an operating portion force state), which may also be referred to as an external contact force.

The parameters measured by the six-dimensional force sensor are three-dimensional orthogonal force and three-dimensional orthogonal moment, namely three-way acting force (x-axis force, y-axis force and z-axis force) and acting moment along the three directions, which are applied to the operation device, can be detected. After the three-dimensional normal force measured by the first six-dimensional force sensor is obtained, the operating device stress may be calculated based on the three-dimensional normal force measured by the first six-dimensional force sensor. Specifically, the force applied to the operating device is obtained by calculating the square of the force on the x-axis, the square of the force on the y-axis, and the square of the force on the z-axis, and then calculating the sum of the three squares, and then calculating the square of the sum.

In practical applications, after the first six-dimensional force sensor is mounted on the operation device at the end of the operation arm, as shown in fig. 3, the gravity of the tool at the end of the operation arm may affect the parameter measured by the first six-dimensional force sensor, so that the parameter measured by the first six-dimensional force sensor may not be accurate. Therefore, before the soft endoscope is delivered, gravity compensation needs to be performed on the first six-dimensional force sensor to correct the parameters measured by the first six-dimensional force sensor. In addition, the first six-dimensional force sensor has a three-dimensional force zero value in the absence of any load. The measurement of the first six-dimensional force sensor is also affected by the three-dimensional force zero value.

Based on the above, the embodiment of the application provides a specific implementation manner for acquiring the three-dimensional orthogonal force measured by the first six-dimensional force sensor and calculating the stress of the operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor, which comprises the following steps:

a1: three-dimensional orthogonal forces measured by the first six-dimensional force sensor are acquired.

The three-dimensional normal force (three-way force) measured directly by the first six-dimensional force sensor may be denoted as F _x,F_y,F_z. Where F _x is the force on the x-axis, F _y is the force on the y-axis, and F _z is the force on the z-axis.

A2: and correcting the three-dimensional orthogonal force measured by the first six-dimensional force sensor based on a three-dimensional orthogonal force correction formula to obtain corrected three-dimensional orthogonal force.

A3: and calculating the stress of the operating device based on the corrected three-dimensional orthogonal force.

The three-dimensional orthogonal force correction formula is a formula after the gravity compensation of the first six-dimensional force sensor, and comprises a three-dimensional force zero value of the first six-dimensional force sensor, a gravity three-dimensional orthogonal force and a three-dimensional orthogonal force measured by the first six-dimensional force sensor.

Based on a three-dimensional orthogonal force correction formula, the three-dimensional orthogonal force measured by the first six-dimensional force sensor can be corrected by utilizing the three-dimensional force zero value of the first six-dimensional force sensor and the gravity three-dimensional orthogonal force, so as to obtain corrected three-dimensional orthogonal force. Further, the operating device force can be calculated based on the corrected three-dimensional orthogonal force.

Based on the content of A1-A3, after the gravity compensation is performed on the first six-dimensional force sensor, the stress of the operation device calculated based on the corrected three-dimensional orthogonal force is more accurate.

In one possible implementation manner, the embodiment of the present application provides a specific implementation manner for performing gravity compensation on a first six-dimensional force sensor to obtain a three-dimensional orthogonal force correction formula, which specifically includes the following steps:

A21: a pose transformation matrix from the robot world coordinate system to the first six-dimensional force sensor coordinate system is calculated.

In a state where the soft endoscope operation section is mounted on the operation device, a posture conversion matrix from the robot world coordinate system to the first six-dimensional force sensor coordinate system is calculated:

wherein, R is the pose of the operating arm, and can be composed of the space position and the pose of the operating arm. The robot world coordinate system is { O }, the first six-dimensional force sensor coordinate system is { S }, the flange coordinate system is { W }, and the manipulator base coordinate system of the manipulator is { B }.

A22: decomposing the gravity of the tool at the tail end of the operating arm under the robot world coordinate system into a first six-dimensional force sensor coordinate system based on the gesture transformation matrix to obtain a target formula; the target formula comprises three-dimensional orthogonal force measured by the first six-dimensional force sensor, gravity three-dimensional orthogonal force, three-dimensional force zero value of the first six-dimensional force sensor and an installation inclination angle of the operating arm.

Based on the gesture transformation matrix, decomposing the gravity of the manipulator end tool under the robot world coordinate system { O } into a first six-dimensional force sensor coordinate system { S }, and obtaining a target formula:

G _x,g_y,g_z is the three-dimensional orthogonal force of gravity after the gravity g of the tool at the tail end of the operating arm is decomposed, F _x0,F_y0,F_z0 is the zero value of the three-dimensional force of the first six-dimensional force sensor, and U and V are the installation inclination angles of the operating arm.

A23: acquiring six-dimensional force sensor data of a first six-dimensional force sensor under a plurality of different postures of an operation arm, and calculating a three-dimensional force zero value and a gravity three-dimensional orthogonal force of the first six-dimensional force sensor based on the six-dimensional force sensor data and a target formula; six-dimensional force sensor data includes three-dimensional orthogonal forces and three-dimensional orthogonal moments.

After the target formula is acquired, six-dimensional force sensor data of the first six-dimensional force sensor in a plurality of different postures of the operation arm are measured. The measured six-dimensional force sensor data are substituted into a target formula, so that the three-dimensional force zero point value and the gravity three-dimensional orthogonal force of the first six-dimensional force sensor can be calculated, and the installation inclination angle of the operating arm can be calculated. Thereby, gravity compensation is completed.

Wherein the gravity three-dimensional orthogonal force can be used to calculate the manipulator end-of-arm tool gravity g.

A24: and acquiring a three-dimensional orthogonal force correction formula based on the three-dimensional force zero value of the first six-dimensional force sensor, the gravity three-dimensional orthogonal force and the three-dimensional orthogonal force measured by the first six-dimensional force sensor.

After the three-dimensional force zero point value and the gravity three-dimensional orthogonal force of the first six-dimensional force sensor are obtained, a three-dimensional orthogonal force correction formula can be obtained, as follows:

wherein the corrected three-dimensional orthogonal force is F _xe,F_ye,F_ze.

In addition, a three-dimensional orthogonal moment correction formula can be obtained as follows:

The corrected three-dimensional orthogonal moment is M _xe,M_ye,M_ze.M_x,M_y,M_z, which is the three-dimensional orthogonal moment directly measured by the first six-dimensional force sensor. M _x0,M_y0,M_z0 is the moment corresponding to the three-dimensional force zero value of the first six-dimensional force sensor, and M _gx,M_gy,M_gz is the moment corresponding to the three-dimensional orthogonal force through gravity.

S203: when the stress of the operating device is a tensile force and the stress of the operating device reaches a tension threshold of the soft endoscope, updating the next preselected track point to be a first track point, and controlling the operating arm according to the first track point; in the preset track, a first number of track points are spaced between the first track point and the next preselected track point, and the distance between the first track point and the conveying device at the tail end of the conveying arm is smaller than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

In practical applications, the force applied to the operating device may be either a tensile force or a compressive force. The sign of the stress of the operation device obtained by the embodiment of the application has positive and negative conditions. In this case, the sign of the operating device stress is a positive sign, and the sign of the operating device stress is a negative sign, and the operating device stress is a pressure. After the stress of the operating device is obtained, whether the stress of the operating device is tensile or compressive can be judged according to the sign of the stress of the operating device.

When the sign of the force applied to the operating device is positive, the sign indicates that the force applied to the operating device is tensile, and the sign also indicates that the force applied to the soft endoscope is tensile. And comparing the stress of the operation device with a tension threshold of the soft endoscope, wherein the tension threshold of the soft endoscope can be obtained in advance. When the stress of the operation device reaches the tension threshold of the soft endoscope, the tension applied to the soft endoscope is larger, and the soft endoscope can be damaged. At this time, it is necessary to alleviate the tensile force applied to the soft endoscope.

Specifically, in order to alleviate the tensile force applied to the soft endoscope, the next preselected track point may be updated to a first track point, and the operation arm may be controlled according to the first track point. In the preset track, a first number of track points are spaced between the first track point and the next preselected track point, and the distance between the first track point and the conveying device at the tail end of the conveying arm is smaller than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm. Wherein the first number may be one or more, and is not limited herein. In this way, the next preselected track point is moved forward, the track point position of the operation arm is adjusted to the direction approaching the conveying device, and the redefined track point is the first track point. Therefore, the tension applied to the soft endoscope can be slowed down, the soft endoscope is prevented from being damaged as much as possible, the accurate regulation and control of the following position of the operation arm is realized, and the quick conveying of the soft endoscope can be facilitated.

In the embodiment of the application, all track points in the preset track are reachable, namely, when the control operation arm reaches each track point, the motors in the mechanical arm do not exceed the limit angle. The limiting angle is a limiting angle of the mechanical arm joint, for example, the limiting angle can be +/-120 degrees.

In one possible implementation manner, the embodiment of the application provides a specific implementation manner for acquiring the tension threshold value of the soft endoscope, which comprises the following steps:

b1: and controlling the conveying arm and the operating arm to stretch the soft endoscope until the state of the soft endoscope is the maximum tension state.

Before the soft endoscope is not delivered, the delivery arm and the operating arm are controlled to stretch the soft endoscope. When the soft endoscope is stretched until the soft endoscope is in a slipping state, the soft endoscope can be considered to reach a maximum tension state.

B2: and calculating the maximum tensile force of the operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor when the state of the soft endoscope is the maximum tensile state.

When the soft endoscope reaches the maximum tension state, the three-dimensional orthogonal force measured by the first six-dimensional force sensor at the moment is obtained, so that the corrected three-dimensional orthogonal force can be calculated based on a three-dimensional orthogonal force correction formula, and the maximum tension of the operating device can be calculated based on the corrected three-dimensional orthogonal force. The maximum tensile force is the peak value of the tensile force of the soft endoscope.

B3: and determining the product of the maximum tensile force of the operating device and the second preset proportion as a tension threshold of the soft endoscope.

It is understood that the second preset proportion is not limited herein, and for example, the second preset proportion may be 65%. After determining the tension threshold of the soft endoscope, when the operating device is stressed as a tensile force, an adjustment reference can be provided for the locus point of the operating arm based on whether the operating device is stressed to reach the tension threshold of the soft endoscope.

Based on the above-mentioned related content of S201-S203, it can be known that, according to the embodiment of the present application, based on the preset track (composed of a plurality of track points) of the existing operation arm, accurate adjustment and control on the following position of the operation arm can be achieved based on the six-dimensional force sensor. Wherein the first six-dimensional force sensor is mounted on the operating device at the end of the operating arm. In specific implementation, the next preselected track point of the operation arm is obtained firstly based on the preset track of the operation arm. But the next preselected trajectory point determined may be an inaccurate trajectory point. At this time, the three-dimensional orthogonal force measured by the first six-dimensional force sensor is obtained, and the stress of the operating device is calculated based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor, wherein the stress of the operating device is the tension or compression force of the soft endoscope. If the calculated current operating device stress is a tensile force and the operating device stress reaches the soft endoscope tensile force threshold, the soft endoscope stress is a tensile force and the soft endoscope tensile force exceeds the soft endoscope tensile force threshold. That is, the soft endoscope receives a large tensile force. At this time, the next preselected track point is updated to the first track point. The first trajectory point is a trajectory point closer to the conveyor in the preset trajectory than the next preselected trajectory point. Further, by controlling the operation arm according to the first locus point, the situation that the tension of the soft endoscope is large can be relieved. Therefore, on the basis of the original preset track, the accurate regulation and control of the following position of the operation arm is realized, and the soft endoscope conveying can be completed quickly.

Referring to fig. 4, fig. 4 is a schematic diagram of a control method of a mechanical arm based on a six-dimensional force sensor according to an embodiment of the present application. In another possible implementation manner, referring to fig. 4, the method for controlling a mechanical arm based on a six-dimensional force sensor according to the embodiment of the present application may further include the following steps:

When the stress of the operating device is pressure and the stress of the operating device reaches a soft endoscope stress threshold value, updating the next preselected track point into a second track point; in the preset track, a second number of track points are spaced between the second track point and the next preselected track point, and the distance between the second track point and the conveying device at the tail end of the conveying arm is larger than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

For example, when the sign of the force applied to the operation device is a negative sign, it indicates that the force applied to the operation device is a pressure (extrusion force), and also indicates that the force applied to the soft endoscope is a pressure. And comparing the stress of the operation device with a soft endoscope stress threshold value, wherein the soft endoscope stress threshold value can be obtained in advance. When the stress of the operating device reaches the pressure threshold of the soft endoscope, the pressure applied to the soft endoscope is larger, and the soft endoscope can be damaged. At this time, it is necessary to relieve the pressure applied to the soft endoscope.

Specifically, in order to alleviate the pressure exerted by the soft endoscope, the next preselected trajectory point may be updated to a second trajectory point, and the operating arm may be controlled in accordance with the second trajectory point. In the preset track, a second number of track points are spaced between the second track point and the next preselected track point, and the distance between the second track point and the conveying device at the tail end of the conveying arm is larger than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm. Wherein the second number may be one or more, and is not limited herein. In this way, the next preselected track point is moved backward, the track point position of the operation arm is adjusted in a direction away from the conveying device, and the redefined track point is the second track point. Therefore, the pressure applied to the soft endoscope can be relieved, the soft endoscope can be prevented from being damaged as much as possible, the accurate regulation and control of the following position of the operation arm is realized, and the soft endoscope conveying device is beneficial to quickly completing conveying of the soft endoscope.

In one possible implementation manner, the embodiment of the application provides a specific implementation manner for acquiring the stress threshold value of the soft endoscope, which comprises the following steps:

C1: the delivery arm and the operating arm are controlled to compress the soft endoscope until the state of the soft endoscope is the maximum compression state.

The delivery arm and the operating arm are controlled to compress the soft endoscope before the soft endoscope is delivered. When the soft endoscope is compressed until the soft endoscope is in a slipping state, the soft endoscope can be considered to reach a maximum compression state.

C2: the maximum stress of the operating device is calculated based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor when the state of the soft endoscope is the maximum stress state.

When the soft endoscope reaches the maximum compression state, the three-dimensional orthogonal force measured by the first six-dimensional force sensor at the moment is obtained, so that the corrected three-dimensional orthogonal force can be calculated based on a three-dimensional orthogonal force correction formula, and the maximum compression force of the operating device can be calculated based on the corrected three-dimensional orthogonal force. The maximum stress is the peak value of the stress of the soft endoscope.

And C3: the product of the maximum pressure of the operating device and the third preset proportion is determined as a soft endoscope pressure threshold value.

It is understood that the third preset proportion is not limited herein, and for example, the third preset proportion may be 65%. After determining the soft endoscope compression threshold, when the operating device is under compression, an adjustment reference can be provided to the locus of the operating arm by whether the soft endoscope compression threshold reaches the soft endoscope compression threshold.

Illustratively, the soft endoscopic robotic manipulator system further includes a second six-dimensional force sensor (which may also be referred to as a delivery portion six-dimensional force sensor) mounted at the distal end of the delivery arm. Referring to fig. 5, fig. 5 is a schematic installation diagram of a second six-dimensional force sensor according to an embodiment of the present application. As shown in fig. 5, a second six-dimensional force sensor is mounted between the distal end of the delivery arm and the delivery device. The coordinate system of the second six-dimensional force sensor is shown in fig. 5, the z-axis direction is perpendicular to the conveying direction of the soft endoscope, the y-axis direction is downward, and the x-axis direction is outward. The coordinate system of the conveying device is shown in fig. 5, the z-axis direction is the conveying direction of the soft endoscope, the x-axis direction is downward, and the y-axis direction is outward. It is known that the coordinate system of the second six-dimensional force sensor and the coordinate system of the conveying device are all cartesian coordinate systems, and the right-hand rule is met.

In the embodiment of the application, based on the second six-dimensional force sensor, the accurate adjustment of the conveying speed of the soft endoscope can be realized.

Specifically, with reference to fig. 4, the mechanical arm control method based on the six-dimensional force sensor provided by the embodiment of the application further includes D1-D3:

D1: and acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor, and calculating the stress of the conveying device based on the three-dimensional orthogonal force measured by the second six-dimensional force sensor.

Specifically, the square of the x-axis force, the square of the y-axis force and the square of the z-axis force in the three-dimensional orthogonal force measured by the second six-dimensional force sensor are calculated first, and then the sum of the three squares is calculated, and the square of the sum is calculated, so that the stress of the conveying device can be obtained. In practical application, the conveying device is stressed in the z-axis direction of the second six-dimensional force sensor coordinate system in a smaller and negligible manner. Therefore, the square of the x-axis force and the square of the y-axis force in the three-dimensional orthogonal force can be calculated, the sum of the two squares is calculated, and then the convenience of the sum is calculated, so that the stress of the conveying device can be obtained.

The force applied to the conveying device can represent the force applied state (namely the force applied state of the conveying part) of the conveying device. As shown in fig. 4, in the process of delivering the soft endoscope, the force applied to the delivering device includes the resistance of the human body and the internal force of the endoscope. Because the second six-dimensional force sensor is arranged at the tail end of the conveying arm, the tension or compression force (namely the internal force of the endoscope) of the soft endoscope can be transmitted to the conveying device, and then the force is detected by the second six-dimensional force sensor. The human body resistance is the reaction force of the natural cavity channel of the human body to the soft endoscope when the soft endoscope is input into the natural cavity channel of the human body. The reaction force is conducted to the conveying device after acting on the soft endoscope, and then detected by the second six-dimensional force sensor.

In one possible implementation manner, the embodiment of the application provides a specific implementation manner for calculating the stress of the conveying device based on the three-dimensional orthogonal force measured by the second six-dimensional force sensor by acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor, which comprises the following steps of D11-D13:

D11: and acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor, correcting the three-dimensional orthogonal force measured by the second six-dimensional force sensor based on a three-dimensional orthogonal force correction formula, and acquiring the corrected three-dimensional orthogonal force.

In the embodiment of the present application, the gravity compensation may be performed on the second six-dimensional force sensor based on the three-dimensional orthogonal force correction formula, and the specific process is similar to the gravity compensation of the first six-dimensional force sensor, which is not repeated herein, and reference may be made to the above embodiment.

D12: and calculating the stress of the conveying device based on the corrected three-dimensional orthogonal force.

And after the corrected three-dimensional orthogonal force is obtained, calculating the stress of the operating device based on the corrected three-dimensional orthogonal force.

D13: and compensating the force deviation value of the stress of the conveying device to obtain the compensated stress of the conveying device.

In practical applications, the second six-dimensional force sensor may have measurement bias. Based on the above, the force deviation value compensation is carried out on the stress of the conveying device after the stress of the conveying device is obtained, and the compensated stress of the conveying device is obtained. Wherein the force bias value may be obtained before the soft endoscope is delivered. Therefore, after the gravity compensation and the force deviation value compensation are carried out on the second six-dimensional force sensor, the compensated stress of the conveying device can be more accurate.

Specifically, the sum of the conveyor force and the force deviation value is determined as the compensated conveyor force.

The embodiment of the application provides a specific implementation method for acquiring a force deviation value, which comprises the following steps of D131-D132:

D131: and controlling the conveying device to convey the soft endoscope to the direction of the pressure detection device until the soft endoscope reaches a slipping state, and acquiring the maximum actual stress of the conveying device detected by the pressure detection device and the maximum actual stress of the conveying device based on the second six-dimensional force sensor.

Specifically, a pressure detecting device is placed at the head end portion of the soft endoscope, and the head end portion of the soft endoscope is abutted against the pressure detecting device.

Further, the soft endoscope is conveyed to the direction of the pressure detection device by a motor in the conveying device until the soft endoscope reaches a slipping state. When the soft endoscope reaches a slipping state, the motor conveying force is maximum. It is known that the pressure detecting device generates a reaction force after the soft endoscope is abutted against the pressure detecting device, and the reaction force is equivalent to the conveying force of the motor. Moreover, the reaction force can act on the soft endoscope, and the stress of the soft endoscope can be transmitted to the conveying device, so that the conveying device is stressed, and the stress of the conveying device can be detected by the second six-dimensional force sensor.

It can be understood that in the process of calibrating the force deviation value, no human body resistance and endoscope stress exist, and only the reaction force of the pressure detection device exists.

Based on the above, when the soft endoscope reaches a slipping state, the force of the conveying device obtained by the second six-dimensional force sensor is the maximum actually measured force of the conveying device. And the delivery force/reaction force detected by the pressure detection means at this time may be regarded as the maximum actual force of the delivery means.

The maximum actually measured stress of the conveying device obtained by the second six-dimensional force sensor is also obtained after the gravity compensation of the second six-dimensional force sensor.

D132: and determining the difference value between the maximum actual stress of the conveying device and the maximum actual stress of the conveying device as a force deviation value.

Here, the maximum actual force of the conveying device may be considered as a true value, and the maximum actual force of the conveying device may be considered as an actual measured value, whereby a difference between the two may be calculated, and the difference may be determined as a force deviation value. The force deviation value is used for realizing the compensation of the stress of the conveying device.

In addition, the maximum actual stress of the conveying device can be considered as the maximum value of the stress of the conveying device, and the range of the stress of the conveying device is 0 to the maximum actual stress of the conveying device, and the stress threshold of the conveying device can be determined based on the maximum actual stress of the conveying device later, and particularly, the following description is given.

D2: when the stress of the conveying device is increased and the stress of the operating device does not reach the tension threshold of the soft endoscope, the target resistance of the soft endoscope is determined, and the conveying speed of the conveying device to the soft endoscope and the moving speed of the operating arm are reduced.

It is understood that the delivery device forces include body resistance and endoscopic forces. When the stress of the conveying device is increased and the stress of the operating device does not reach the tension threshold of the soft endoscope, the soft endoscope is indicated to be subjected to human body resistance, and the human body resistance can be increased. In order to avoid damage to the natural cavity of the human body, the conveying speed of the conveying device to the soft endoscope is reduced so as to reduce the conveying length of the soft endoscope. Since the operation arm follows the movement of the soft endoscope during the process of transporting the soft endoscope, the movement speed of the operation arm is also required to be reduced. In this way, the precise adjustment of the conveying speed of the conveying device and the precise adjustment of the movement speed of the operating arm can be performed based on the magnitude of the force applied by the conveying device.

D3: when the stress of the conveying device is larger than the stress threshold of the conveying device and the stress of the operating device does not reach the tension threshold of the soft endoscope, the conveying device is controlled to stop conveying the soft endoscope.

After the maximum actual stress of the conveying device is obtained, the product of the maximum actual stress of the conveying device and the first preset proportion can be determined as a stress threshold value of the conveying device. For example, the first preset ratio is 65%, which is not limited herein.

When the stress of the conveying device is larger than the stress threshold of the conveying device and the stress of the operating device does not reach the tension threshold of the soft endoscope, the inner force of the soft endoscope is smaller, but the stress of the conveying device is larger, so that the soft endoscope can be determined to be subjected to human body resistance and the human body resistance is larger. At this time, the conveying device is directly controlled to stop conveying the soft endoscope, the conveying action of the soft endoscope is forbidden, accidents are avoided, and the conveying safety is improved.

It is understood that when the force applied to the conveying device is smaller than the force threshold of the conveying device and the force applied to the operating device does not reach the tension threshold of the soft endoscope, the operation of the operating arm can be controlled according to the preset track.

Based on D1-D3, based on the first six-dimensional force sensor and the second six-dimensional force sensor which are installed, not only can the following position of the operation arm be accurately regulated and controlled, but also the conveying speed of the soft endoscope can be accurately regulated by the conveying device, the conveying safety is effectively improved, and the damage behavior of the endoscope caused by improper operation in the conveying process can be avoided as much as possible.

In the process of conveying the soft endoscope, if the operation arm and the conveying arm rotate asynchronously, torque along the z axis of the operation device is generated, so that the soft endoscope is rotated. In order to avoid damage to the soft endoscope, in conjunction with fig. 4, the mechanical arm control method based on the six-dimensional force sensor provided by the embodiment of the application may further include the following steps:

e1: the rotational torque and rotational direction of the soft endoscope are obtained.

The rotation torque of the soft endoscope is the z-axis torque of the first six-dimensional force sensor. The rotational direction of the soft endoscope can be obtained by the rotational directions of the transport arm and the operating arm.

E2: when the rotation torque of the soft endoscope is larger than the rotation torque threshold, the control operation arm stops rotating the soft endoscope in the rotation direction.

When the rotation torque of the soft endoscope is determined to be larger than the rotation torque threshold, prompt information can be sent to the control console to prompt that the rotation torque of the soft endoscope is larger than the rotation torque threshold.

As shown in fig. 4, the process of verifying whether the rotation torque of the soft endoscope is greater than the rotation torque threshold according to the embodiment of the present application may be performed between verifying whether the soft endoscope is under compression or under tension, or may be performed simultaneously with verifying whether the soft endoscope is under tension, which is not limited herein.

When the operation arm is controlled to stop rotating the soft endoscope in the rotation direction, the operation arm may be controlled to rotate the soft endoscope in the opposite direction to the rotation direction, so that the rotation state of the soft endoscope is relieved. In addition, the track point of the operation arm can be adjusted, so that the operation arm moves forwards or backwards on the basis of the current track point, the rotating state of the soft endoscope is relieved, and the soft endoscope is prevented from being damaged.

In this step, the rotational torque threshold value may be obtained in advance. In one possible implementation, the embodiment of the present application provides a specific implementation for acquiring a rotation torque threshold, including E21-E22:

e21: and obtaining the maximum stress moment of the operating device.

In one possible implementation, the maximum force moment of the operating device can be obtained by the following method, specifically including F1-F2:

F1: and controlling the operation arm to rotate the soft endoscope until the state of the soft endoscope is a slipping state, and acquiring the three-dimensional orthogonal moment measured by the first six-dimensional force sensor.

It will be appreciated that when the state of the soft endoscope is a slip state, it is indicative that the soft endoscope has reached a maximum rotation state.

F2: and calculating the maximum stress moment of the operating device based on the three-dimensional orthogonal moment.

Specifically, the square of the moment in the x-axis direction, the square of the moment in the y-axis direction, and the square of the moment in the z-axis direction are calculated first, and the sum of the three squares is calculated. Furthermore, the maximum force moment of the operating device can be obtained by calculating the sum.

In another possible implementation manner, the maximum stress moment of the operating device can be obtained by the following method, specifically including G1-G3:

G1: the control operation device and the conveying device are positioned on the same horizontal plane.

Specifically, the control operation device and the conveying device are positioned on the same horizontal plane so that the soft endoscope is parallel to the ground. At this time, the operating device is forced only in the z-axis direction.

And G2: when the operating device and the conveying device are positioned on the same horizontal plane, the operating arm is controlled to rotate the soft endoscope until the state of the soft endoscope is a slipping state, and the z-axis moment measured by the first six-dimensional force sensor is obtained.

When the state of the soft endoscope is a slipping state, the z-axis moment measured by the first six-dimensional force sensor is the largest. It is understood that the first six-dimensional force sensor in this step may be a gravity compensated sensor. Therefore, after the z-axis moment measured by the first six-dimensional force sensor is obtained, the z-axis moment can be corrected, and the corrected z-axis moment is obtained.

And G3: the z-axis moment is determined as the maximum force moment of the operating device.

When the operating device and the conveying device are positioned on the same horizontal plane, the operating device is only stressed in the z-axis direction, and the maximum z-axis moment in the G2 step can be determined as the maximum stress moment of the operating device.

E22: and determining the product of the maximum stress moment of the operating device and the fourth preset proportion as a rotation torque threshold value of the soft endoscope.

It is understood that the fourth preset proportion is not limited herein, and for example, the fourth preset proportion may be 65%. Thus, the rotational torque threshold of the soft endoscope can be calculated.

It will be appreciated by those skilled in the art that in the above-described method of the specific embodiments, the written order of steps is not meant to imply a strict order of execution but rather should be construed according to the function and possibly inherent logic of the steps.

Based on the method provided by the embodiment of the method, the embodiment of the application also provides a mechanical arm control device based on the six-dimensional force sensor, and the mechanical arm control device based on the six-dimensional force sensor is described below with reference to the accompanying drawings. Because the principle of solving the problem of the device in the embodiment of the present disclosure is similar to that of the mechanical arm control method based on the six-dimensional force sensor in the embodiment of the present disclosure, the implementation of the device may refer to the implementation of the method, and the repetition is not repeated.

Referring to fig. 6, the structure of a mechanical arm control device based on a six-dimensional force sensor according to an embodiment of the present application is shown. The device is applied to a soft endoscope operation robot system, and the soft endoscope operation robot system comprises a conveying arm, an operation arm and a first six-dimensional force sensor; the first six-dimensional force sensor is mounted on the operating device at the end of the operating arm. As shown in fig. 6, the six-dimensional force sensor-based robot arm control device 600 includes:

a first acquiring unit 601, configured to acquire a next preselected trajectory point of the operation arm; the next preselected track point belongs to a track point in a preset track of the operation arm;

a second obtaining unit 602, configured to obtain a three-dimensional orthogonal force measured by the first six-dimensional force sensor, and calculate an operating device stress based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor; the operating device is stressed by the tension or compression force of the soft endoscope;

a first updating unit 603, configured to update the next preselected trajectory point to a first trajectory point when the operating device is under tension and reaches a tension threshold of the soft endoscope, and control the operating arm according to the first trajectory point; in the preset track, a first number of track points are spaced between the first track point and the next preselected track point, and the distance between the first track point and the conveying device at the tail end of the conveying arm is smaller than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

In one possible implementation, the apparatus further includes:

the second updating unit is used for updating the next preselected track point into a second track point when the operating device is stressed to be pressure and the operating device is stressed to reach a soft endoscope stressed threshold;

In the preset track, a second number of track points are spaced between the second track point and the next preselected track point, and the distance between the second track point and the conveying device at the tail end of the conveying arm is larger than the distance between the next preselected track point and the conveying device at the tail end of the conveying arm.

In one possible implementation, the soft endoscopic surgical robotic system further comprises a second six-dimensional force sensor; the second six-dimensional force sensor is arranged at the tail end of the conveying arm; the apparatus further comprises:

the third acquisition unit is used for acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor and calculating the stress of the conveying device based on the three-dimensional orthogonal force measured by the second six-dimensional force sensor;

A determining unit, configured to determine that the soft endoscope receives a target resistance when the force applied to the conveying device increases and the force applied to the operating device does not reach the tension threshold of the soft endoscope, and reduce a conveying speed of the conveying device to the soft endoscope and a movement speed of the operating arm;

The first control unit is used for controlling the conveying device to stop conveying the soft endoscope when the stress of the conveying device is larger than the stress threshold of the conveying device and the stress of the operating device does not reach the tension threshold of the soft endoscope;

The stress threshold of the conveying device is the product of the maximum actual stress of the conveying device and a first preset proportion.

In one possible implementation manner, the second obtaining unit 602 includes:

the first acquisition subunit is used for acquiring the three-dimensional orthogonal force measured by the first six-dimensional force sensor;

The correction subunit is used for correcting the three-dimensional orthogonal force measured by the first six-dimensional force sensor based on a three-dimensional orthogonal force correction formula to obtain corrected three-dimensional orthogonal force;

a first computing subunit for computing an operating device force based on the corrected three-dimensional orthogonal force.

In one possible implementation, the process of obtaining the three-dimensional orthogonal force correction formula includes:

Calculating an attitude transformation matrix from a robot world coordinate system to a first six-dimensional force sensor coordinate system;

Decomposing the gravity of the tool at the tail end of the operating arm under the robot world coordinate system into a first six-dimensional force sensor coordinate system based on the gesture transformation matrix to obtain a target formula; the target formula comprises a three-dimensional orthogonal force measured by the first six-dimensional force sensor, a gravity three-dimensional orthogonal force, a three-dimensional force zero value of the first six-dimensional force sensor and an operating arm installation inclination angle;

Acquiring six-dimensional force sensor data of the first six-dimensional force sensor under a plurality of different postures of an operation arm, and calculating a three-dimensional force zero value of the first six-dimensional force sensor and the gravity three-dimensional orthogonal force based on the six-dimensional force sensor data and the target formula; the six-dimensional force sensor data comprise three-dimensional orthogonal force and three-dimensional orthogonal moment;

and acquiring a three-dimensional orthogonal force correction formula based on the three-dimensional force zero value of the first six-dimensional force sensor, the gravity three-dimensional orthogonal force and the three-dimensional orthogonal force measured by the first six-dimensional force sensor.

In one possible implementation, the process for obtaining the tension threshold of the soft endoscope includes:

controlling the conveying arm and the operating arm to stretch the soft endoscope until the state of the soft endoscope is a maximum tension state;

Calculating the maximum tensile force of an operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor when the state of the soft endoscope is the maximum tensile state;

and determining the product of the maximum tensile force of the operating device and a second preset proportion as a tension threshold of the soft endoscope.

In one possible implementation, the process of acquiring the pressure threshold of the soft endoscope includes:

Controlling the conveying arm and the operating arm to compress the soft endoscope until the state of the soft endoscope is the maximum compression state;

calculating the maximum stressed force of an operating device based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor when the state of the soft endoscope is the maximum stressed state;

and determining the product of the maximum pressure of the operating device and a third preset proportion as a soft endoscope pressure threshold value.

In one possible implementation, the soft endoscope has a pressure detection device at the head end; the third acquisition unit includes:

The second acquisition subunit is used for acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor, correcting the three-dimensional orthogonal force measured by the second six-dimensional force sensor based on a three-dimensional orthogonal force correction formula, and acquiring the corrected three-dimensional orthogonal force;

The second calculating subunit is used for calculating the stress of the conveying device based on the corrected three-dimensional orthogonal force;

and the compensation subunit is used for compensating the force deviation value of the stress of the conveying device and obtaining the compensated stress of the conveying device.

In one possible implementation, the process of obtaining the force deviation value includes:

Controlling a conveying device to convey the soft endoscope to the direction of the pressure detection device until the soft endoscope reaches a slipping state, and acquiring the maximum actual stress of the conveying device detected by the pressure detection device and the maximum actual measured stress of the conveying device obtained based on the second six-dimensional force sensor;

and determining the difference value between the maximum actual stress of the conveying device and the maximum actual stress of the conveying device as a force deviation value.

In one possible implementation, the apparatus further includes:

a fourth acquisition unit configured to acquire a rotation torque and a rotation direction of the soft endoscope;

And the second control unit is used for controlling the operation arm to stop rotating the soft endoscope in the rotating direction when the rotating torque of the soft endoscope is larger than a rotating torque threshold value.

In one possible implementation, the process of obtaining the rotation torque threshold includes:

Obtaining the maximum stress moment of the operating device;

and determining the product of the maximum stress moment of the operating device and a fourth preset proportion as a rotation torque threshold value of the soft endoscope.

In one possible implementation manner, the acquiring the maximum stress moment of the operating device includes:

controlling the operation arm to rotate the soft endoscope until the state of the soft endoscope is a slipping state, and acquiring the three-dimensional orthogonal moment measured by the first six-dimensional force sensor;

calculating the maximum stress moment of the operating device based on the three-dimensional orthogonal moment;

Or alternatively

Controlling the operating device and the conveying device to be positioned on the same horizontal plane;

When the operating device and the conveying device are positioned on the same horizontal plane, the operating arm is controlled to rotate the soft endoscope until the state of the soft endoscope is a slipping state, and the z-axis moment measured by the first six-dimensional force sensor is obtained;

The z-axis moment is determined as the maximum force moment of the operating device.

It should be noted that, for specific implementation of each unit in this embodiment, reference may be made to the related description in the above method embodiment. The division of the units in the embodiment of the application is schematic, only one logic function is divided, and other division modes can be adopted in actual implementation. The functional units in the embodiment of the application can be integrated in one processing unit, or each unit can exist alone physically, or two or more units are integrated in one unit. The integrated units may be implemented in hardware or in software functional units.

From the above description of embodiments, it will be apparent to those skilled in the art that all or part of the steps of the above described example methods may be implemented in software plus necessary general purpose hardware platforms. Based on such understanding, the technical solution of the present application may be embodied essentially or in a part contributing to the prior art in the form of a software product, which may be stored in a storage medium, such as ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for causing a computer device (which may be a personal computer, a server, or a network communication device such as a media gateway, etc.) to execute the method described in the embodiments or some parts of the embodiments of the present application.

It should be noted that, in the present description, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different manner from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the method disclosed in the embodiment, since it corresponds to the system disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the system part.

It should also 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.

The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the application. Thus, the present application is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. The mechanical arm control method based on the six-dimensional force sensor is characterized by being applied to a soft endoscope operation robot system, wherein the soft endoscope operation robot system comprises a conveying arm, an operation arm and a first six-dimensional force sensor; the first six-dimensional force sensor is arranged on an operation device at the tail end of the operation arm; the method comprises the following steps:

2. The method according to claim 1, wherein the method further comprises:

When the operating device is stressed to be pressure and the operating device is stressed to reach a soft endoscope stressed threshold value, updating the next preselected track point to be a second track point;

3. The method of claim 1, wherein the soft endoscopic robotic system further comprises a second six-dimensional force sensor; the second six-dimensional force sensor is arranged at the tail end of the conveying arm; the method further comprises the steps of:

acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor, and calculating the stress of the conveying device based on the three-dimensional orthogonal force measured by the second six-dimensional force sensor;

When the stress of the conveying device is increased and the stress of the operating device does not reach the tension threshold of the soft endoscope, determining that the soft endoscope receives target resistance, and reducing the conveying speed of the conveying device to the soft endoscope and the movement speed of the operating arm;

When the stress of the conveying device is larger than the stress threshold of the conveying device and the stress of the operating device does not reach the tension threshold of the soft endoscope, the conveying device is controlled to stop conveying the soft endoscope;

4. The method of claim 1, wherein the acquiring the three-dimensional orthogonal force measured by the first six-dimensional force sensor, calculating the operating device force based on the three-dimensional orthogonal force measured by the first six-dimensional force sensor, comprises:

acquiring a three-dimensional orthogonal force measured by the first six-dimensional force sensor;

Correcting the three-dimensional orthogonal force measured by the first six-dimensional force sensor based on a three-dimensional orthogonal force correction formula to obtain corrected three-dimensional orthogonal force;

and calculating the stress of the operating device based on the corrected three-dimensional orthogonal force.

5. The method of claim 4, wherein the process of obtaining the three-dimensional orthogonal force correction formula comprises:

6. The method of any one of claims 1-5, wherein the obtaining of the soft endoscope tension threshold comprises:

7. The method of any one of claims 2-5, wherein the soft endoscope compression threshold acquisition process comprises:

8. A method according to claim 3, wherein the soft endoscope is abutted with a pressure detection device at the head end; the obtaining the three-dimensional orthogonal force measured by the second six-dimensional force sensor, calculating the stress of the conveying device based on the three-dimensional orthogonal force measured by the second six-dimensional force sensor, includes:

Acquiring the three-dimensional orthogonal force measured by the second six-dimensional force sensor, correcting the three-dimensional orthogonal force measured by the second six-dimensional force sensor based on a three-dimensional orthogonal force correction formula, and acquiring corrected three-dimensional orthogonal force;

Calculating the stress of the conveying device based on the corrected three-dimensional orthogonal force;

And compensating the force deviation value of the stress of the conveying device to obtain the compensated stress of the conveying device.

9. The method of claim 8, wherein the process of obtaining the force bias value comprises:

Controlling the conveying device to convey the soft endoscope to the direction of the pressure detection device until the soft endoscope reaches a slipping state, and acquiring the maximum actual stress of the conveying device detected by the pressure detection device and the maximum actual measured stress of the conveying device based on the second six-dimensional force sensor;

10. The method according to claim 1, wherein the method further comprises:

Acquiring the rotation torque and the rotation direction of the soft endoscope;

And when the rotation torque of the soft endoscope is larger than a rotation torque threshold value, controlling the operation arm to stop rotating the soft endoscope in the rotation direction.

11. The method of claim 10, wherein the process of obtaining the rotational torque threshold comprises:

Obtaining the maximum stress moment of the operating device;

12. The method of claim 11, wherein the obtaining the maximum force moment of the operating device comprises:

Or alternatively

13. The mechanical arm control device based on the six-dimensional force sensor is characterized by being applied to a soft endoscope operation robot system, wherein the soft endoscope operation robot system comprises a conveying arm, an operation arm and a first six-dimensional force sensor; the first six-dimensional force sensor is arranged on an operation device at the tail end of the operation arm; the device comprises:

14. An electronic device, comprising:

one or more processors;

a storage device having one or more programs stored thereon,

The one or more programs, when executed by the one or more processors, cause the one or more processors to implement the six-dimensional force sensor based robotic arm control method of any of claims 1-12.

15. A computer-readable storage medium, characterized in that a computer program is stored thereon, which computer program, when being executed by a processor, implements the six-dimensional force sensor based robotic arm control method according to any one of claims 1-12.