Method for determining radius of input shaft, flexible arm robot and control method thereof

Abstract

The present application provides a method for determining the radius of an input shaft, a flexible-arm robot, and a control method. The method for determining the radius of the input shaft includes: determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft; determining a compensation relationship according to the change amount of the traction wire, the compensation length of the driving rope, and the change amount of the driving rope; wherein the compensation length is the change amount of the spatial length of the driving rope; determining the functional relationship between the radius of the input shaft and the rotation angle of the input shaft according to the compensation relationship; determining the radius of the input shaft according to the functional relationship. By setting an input shaft with a variable radius, the change in the radius of the input shaft just offsets the change in the spatial length of the driving rope, making the change amount of the length of the traction wire and the rotation angle of the input shaft a linear relationship, effectively ensuring that the driving rope moves to the correct position in space, thereby guaranteeing the motion accuracy of the driving rope and improving the control precision of the continuum flexible-arm robot.

Description

Technical Field

[0001] The present application relates to the technical field of flexible arm robots, and particularly relates to a method for determining the radius of an input shaft, a flexible arm robot, and a control method. Background Art

[0002] Surgical robots based on continuum flexible arms are being increasingly widely used in minimally invasive surgery due to their small incisions and flexible movements. Volume and mass, as important factors affecting the convenience of using surgical robots, have also become the direction of design improvement. However, for traditional flexible arm surgical robots driven by nitinol traction wires, due to the non-bendable characteristics of nitinol traction wires, a matching lead screw drive needs to be arranged, which will increase the final volume and mass.

[0003] Continuum flexible arm surgical robots based on proximal wire rope drives have obvious advantages in reducing system size and mass. That is, the winch input shaft drives the nitinol traction wire of the proximal disk through the wire rope, and drives the distal disk through the proximal disk. However, due to the spatial layout characteristics between the wire rope input shaft and the proximal disk, the length of the spatial segment between the winch and the nitinol traction wire changes as the winch rotates, bringing a certain non-linearity to the drive control amount and affecting the use accuracy of the continuum flexible arm. Summary of the Invention

[0004] The present application provides a method for determining the radius of an input shaft, a flexible arm robot, and a control method to solve the problem that the use accuracy of a continuum flexible arm robot is relatively low due to the non-linearity of control.

[0005] In a first aspect of the present application, a method for determining the radius of an input shaft is provided. The input shaft is applied in a flexible arm robot, and the flexible arm robot includes the input shaft, a traction wire, a drive rope, and a control unit. The drive rope is wound around the input shaft, and one end of the drive rope away from the input shaft is connected to the traction wire. And the input shaft is a columnar structure with a variable radius. The method includes: determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft; determining a compensation relationship according to the change amount of the traction wire, the compensation length of the drive rope, and the change amount of the drive rope; where the compensation length is the change amount of the spatial length of the drive rope; determining the functional relationship between the radius of the input shaft and the rotation angle of the input shaft according to the compensation relationship; and determining the radius of the input shaft according to the functional relationship.

[0006] Optionally, the compensation relationship is: the change amount of the drive rope is equal to the sum of the change amount of the traction wire and the compensation length of the drive rope.

[0007] Optionally, determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft is: the change amount of the traction wire is proportional to the rotation angle of the input shaft.

[0008] Optionally, the compensation length of the driving rope is: the difference between the spatial lengths of the driving rope before and after rotating a preset angle on the input shaft.

[0009] Optionally, determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft is: determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft and the cross-sectional radius of the input shaft corresponding to the starting point where the driving rope winds around the input shaft.

[0010] A method for determining the radius of an input shaft provided in this application sets a variable-radius input shaft, and the change in the radius just offsets the change in the spatial length of the driving rope. In terms of control, the change amount of the traction wire is linearly proportional to the rotation angle of the input shaft. This makes the change amount of the traction wire and the rotation angle of the input shaft a linear relationship, thereby ensuring the motion accuracy of the traction wire and improving the control accuracy of the continuum flexible arm robot. At the same time, since the driving rope drives the continuum flexible arm surgical robot, the flexible arm robot is smaller in size and lighter in weight, saving production costs on the premise of improving the control accuracy of the flexible body robot.

[0011] A second aspect of this application provides a control method for a flexible arm robot, which is applied to a continuum flexible arm robot. The flexible arm robot includes the input shaft, the traction wire, the driving rope, and the control unit. The driving rope is wound around the input shaft. One end of the driving rope away from the input shaft is connected to the traction wire, and the input shaft is a variable-radius columnar structure. The radius of the input shaft is determined according to the radius determination method described in the first aspect. The control method includes: obtaining the joint space angle of the flexible arm robot; calculating the joint space change amount of the flexible arm robot according to the space angle; determining the current position information of the driving rope according to the space change amount; determining the preset position information of the driving rope according to the functional relationship between the radius of the input shaft and the rotation angle of the input shaft; and updating the current position of the driving rope according to the preset position information and the current position information.

[0012] Optionally, determining the preset position information of the driving rope according to the functional relationship between the radius of the input shaft and the rotation angle of the input shaft specifically includes: obtaining the current rotation angle of the input shaft; substituting the rotation angle into the functional relationship to determine the preset position information of the driving rope.

[0013] Optionally, updating the current position of the drive rope according to the preset position information and the current position information specifically includes: calculating the error between the current position information and the preset position information, where the error includes a coordinate position error and a rotation angle error; determining whether the error reaches the positioning accuracy; if so, updating the current position; if not, determining whether the number of calculations of the error has exceeded a preset number; and updating the current position when the number of calculations of the error exceeds the preset number.

[0014] Optionally, the method further includes: if the number of calculations of the error does not exceed the preset number, calculating the joint space change amount of the flexible arm robot according to the spatial angle.

[0015] The control method of the flexible arm robot provided by this application uses an input shaft with a variable radius structure. During the process of controlling the flexible arm of the robot, the change in the radius offsets the change in the spatial length of the drive rope, enabling accurate positioning of the spatial position of the drive rope, thereby ensuring accurate control of the joints of the continuum flexible arm robot, and further improving the control accuracy of the continuum flexible arm robot.

[0016] A third aspect of this application provides a flexible arm robot, which includes an input shaft, a traction wire, a drive rope, and a control unit. The drive rope is wound around the input shaft, and one end of the drive rope away from the input shaft is connected to the traction wire, and the input shaft is a variable radius cylindrical structure; wherein, the radius of the input shaft is determined according to the radius determination method provided in the first aspect; the control unit is used to control the rotation of the input shaft and execute the control method provided in the second aspect.

[0017] The flexible arm robot provided by this application includes a variable radius input shaft determined according to the radius determination method provided in the first aspect, and the control unit can also execute the control method of the flexible arm robot in the second aspect. Therefore, during the actual operation of the flexible arm robot provided in the embodiments of this application, automatic calibration and update of the current position can be realized, thereby improving the control accuracy of the flexible arm robot. Description of the Drawings

[0018] To more clearly illustrate the technical solutions of this application, the drawings required for use in the embodiments will be briefly introduced below. Obviously, for those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative efforts.

[0019] Figure 1 It is a schematic structural diagram of driving a drive rope and a traction wire by an input shaft in the embodiments of this application;

[0020] Figure 2Schematic diagram of the input shaft structure in the embodiment of the present application;

[0021] Figure 3 is Figure 2 Schematic diagram of the spatial geometric relationship in the input shaft at different tangent points of the driving rope in;

[0022] Figure 4 Flowchart of the method for determining the radius of the input shaft in the embodiment of the present application;

[0023] Figure 5 Schematic diagram of the structure of the flexible arm robot in the embodiment of the present application;

[0024] Figure 6 Flowchart of the control method of the flexible arm robot in the embodiment of the present application;

[0025] Figure 7 Schematic diagram of the process for determining the preset position information in the embodiment of the present application;

[0026] Figure 8 Schematic diagram of the process for judging the error in the embodiment of the present application;

[0027] Figure 9 Flowchart of an example of the control method of the flexible arm robot in the embodiment of the present application. Detailed implementation mode

[0028] The embodiments will be described in detail below, and the examples are shown in the drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementation modes described in the following embodiments do not represent all implementation modes consistent with the present application. They are only examples of systems and methods consistent with some aspects of the present application described in detail in the claims.

[0029] During the working process of the continuum flexible arm robot, the second connecting rope connected to the proximal end of the flexible arm robot is pulled by the first connecting rope wound around the rotating shaft to achieve the pulling effect on the proximal continuum of the flexible body robot. Among them, the first connecting rope is on the rotating shaft. Since the second connecting rope is relatively rigid and cannot be directly wound around the rotating shaft, it is necessary to connect the second connecting rope through the first connecting rope and rotate the first connecting rope on the rotating shaft to stretch the second connecting rope to drive the movement of the proximal continuum of the flexible body.

[0030] The first connecting rope is wound in the threaded groove of the rotating shaft. The rotation angle of the rotating shaft and the amount of the first connecting rope extending or retracting in the threaded groove have a linear relationship. The change amount of the proximal continuum joint is realized by the first connecting rope pulling the second connecting rope of the proximal continuum. However, due to the spatial layout characteristics between the rotating shaft and the proximal continuum, the length of the space segment between the rotating shaft and the second connecting rope changes as the rotating shaft rotates, resulting in a non-linear relationship between the final change amount of the second connecting rope and the rotation angle of the rotating shaft, which is not conducive to controlling the change amount of the second connecting rope.

[0031] To solve the above problems, a method for determining the radius of an output shaft is provided in the technical solution of the embodiment of the present application and is applied to a flexible arm robot. The flexible arm robot includes an input shaft, a traction wire, a driving rope, and a flexible arm. The driving rope is wound around the input shaft and drives the flexible arm to move. One end of the driving rope away from the input shaft is connected to the traction wire, and the input shaft is a variable-radius columnar structure. By setting the input shaft with a variable-radius structure whose radius changes non-linearly with the rotation angle, the change in the radius just offsets the change in the spatial length of the driving rope. Finally, the change amount of the traction wire has a linear change with the rotation angle of the input shaft, which is convenient for the control software to implement and improve the control accuracy. The spatial segment length of the driving rope refers to the length of the driving rope between the traction wire and the input shaft. The variable-radius structure means that in the extending direction of the input shaft, the radius of the input shaft is constantly changing. For example, the radius is gradually decreasing, or the radius is gradually increasing.

[0032] For example Figure 1 the L in 1 and L 2 are the spatial lengths of the driving rope at different rotation angles of the input shaft. The spatial segment length of the driving rope can also be understood as the stretched length of the driving rope between the traction wire and the input shaft.

[0033] It can be understood that the change amount of the driving rope refers to the rotation length of the driving rope relative to the input shaft, including the rope-winding state and the rope-unwinding state of the driving rope. Among them, the rope-winding state of the driving rope refers to the state where the driving rope is pulled and pulls the traction wire, and the rope-unwinding state of the driving rope refers to the state where the driving rope is released. In a specific embodiment, the driving rope is a flexible tungsten wire rope, and the traction wire is an alloy wire. Refer to Figure 1 The distance between the traction wire and the vertical direction of the end of the input shaft is Y, where Y is the initial vertical distance. Specifically, the vertical distance is the distance in the vertical direction from the end of the input shaft around which the driving rope can be wound (the end of the input shaft can also be understood as the end with a cross-sectional position on both sides of the shaft body) to the traction wire. After the input shaft rotates by an angle θ, the spatial length between the driving rope, the traction wire, and the input shaft is L 1, continue to rotate the input shaft so that the input shaft continues to rotate by an angle of dθ. At this time, the rotation angle of the input shaft is θ + dθ, and the spatial length between the driving rope, the traction wire, and the input shaft is L 2 .

[0034] See Figure 1 , as the input shaft rotates by dθ and θ + dθ respectively, the spatial length of the driving rope changes from L 1 to L 2 . It can be understood that during the process of the length of the driving rope changing from L 1 to L 2 , it is in the rope-winding state, and the driving rope pulls the traction wire to move. The following embodiments describe the method for determining the radius of the input shaft in this embodiment in the rope-winding state of the driving rope.

[0035] For example, the traction wire is an alloy wire, specifically a nickel-titanium alloy wire. Nickel-titanium alloy is a shape memory alloy. A shape memory alloy is a special alloy that can automatically restore its plastic deformation to the original shape at a certain specific temperature and has good plasticity. The driving rope is a tungsten wire rope and has a certain flexibility to wind around the input shaft.

[0036] Figure 2 is the structural schematic diagram of the input shaft in the embodiment of the present application.

[0037] See Figure 2 , the radii of the two ends of the input shaft are different. Taking the left-right direction in Figure 2 as an example, the radius of the input shaft decreases from left to right. Figure 2 In 0 , the 0-degree starting point represents the initial position of the driving rope on the input shaft. At this time, the radius between the tangent point of the driving rope and the input shaft and the central axis of the input shaft is R

[0038] Figure 3 is Figure 2 the structural schematic diagram of the spatial triangular structure formed by the tangent points in

[0039] See Figure 1 and Figure 3 , R 1 corresponds to Figure 1 the radius of the input shaft corresponding to the tangent point of the driving rope and the input shaft when the spatial length in 1 is L 2 corresponds to Figure 1 the radius of the input shaft corresponding to the tangent point of the driving rope and the input shaft when the spatial length in 2 is L 1 When the input shaft rotates by an angle of dθ from the state with a spatial length of L 1 state (hereinafter referred to as the L 2state (hereinafter referred to as L 2 state), when L 1 In the state, the radius of the input shaft is R 1 , L 2 In the state, the radius of the input shaft is R 2 . When the input shaft is in the L 1 state, the tangent point of the driving rope and the input shaft is B. As the input shaft rotates by an angle dθ, the tangent point B on the input shaft moves along the cross-sectional circumference to point A. At this time, L 2 In the state, the tangent point of the driving rope and the input shaft is C. Therefore, the change amount of the driving rope as the input shaft rotates is the length of the line segment AC. In addition, since the pitch of the input shaft is generally a constant pitch P, that is, the pitch of the input shaft is unchanged. Therefore, the lead ED length corresponding to the rotation of the angle dθ is Pdθ / 2π, where P is in meters and dθ is in rad.

[0040] Figure 4 Flowchart of a method for determining the radius of an input shaft in an embodiment of the present application.

[0041] See Figure 4 , the method for determining the radius of the input shaft includes:

[0042] Step S11: Determine the change amount of the traction wire according to the change amount of the rotation angle of the input shaft;

[0043] Specifically, during the process of the traction wire being pulled by the driving rope, the input shaft rotates, and then drives the driving rope wound around the input shaft to pull the traction wire to move.

[0044] It can be understood that if the spatial length of the driving rope remains unchanged during the process of the traction wire being pulled by the driving rope (that is, the vertical distance between the tangent point of the driving rope on the input shaft and the traction wire is constant), the change amount of the traction wire is the same as the change amount of the driving rope on the input shaft. Of course, this is an ideal working state. In actual work, the spatial length of the driving rope will change continuously as the input shaft rotates, and the tangent point of the driving rope on the input shaft will also move continuously with the change of the spatial length of the driving rope. In this embodiment, a variable-radius structure of the input shaft is set to compensate for the change amount of the spatial length of the driving rope, so that the change amount of the traction wire and the rotation angle of the input shaft are in a linear relationship. Therefore, the change amount of the traction wire is designed to be the change amount of the driving rope theoretically (when the spatial length of the driving rope remains unchanged and there is no need to compensate for the spatial length), and the change amount of the theoretical driving shaft is R 0 *dθ. That is to say, the designed change amount of the traction wire is R 0 *dθ.

[0045] The change amount of the traction wire is determined according to the change amount of the rotation angle of the input shaft as follows: the change amount of the traction wire is determined according to the change amount of the rotation angle of the input shaft and the cross-sectional radius of the input shaft corresponding to the starting point where the driving rope winds around the input shaft. Among them, the change amount of the rotation angle of the input shaft is dθ, and the cross-sectional radius of the input shaft corresponding to the starting point where the driving rope winds around the input shaft is R 0 .

[0046] It can be seen that the change amount of the traction wire is proportional to the rotation angle of the input shaft. The larger the rotation angle of the input shaft, the larger the change amount of the traction wire

[0047] Step S12: Determine the compensation relationship according to the change amount of the traction wire, the compensation length of the driving rope, and the change amount of the driving rope; among them, the compensation length is the change amount of the spatial length of the driving rope

[0048] Specifically, in step S12, the compensation relationship satisfies: dl = L 2 -L 1 +R 0 *dθ(1);

[0049] Among them, dl is the total elongation length of the input shaft during the process that the spatial length of the driving rope changes from L 1 to L 2 ; R 0 *dθ is the change amount of the elongation of the designed traction wire during the process that the spatial length of the driving rope changes from L 1 to L 2 ; the spatial length L 1 of the driving rope corresponding to the input shaft rotating by θ angle and the spatial length L 2 of the driving rope corresponding to the input shaft rotating by θ + dθ angle, during the process that it changes from L 1 to L 2 , the change amount of the spatial length of the driving rope is L 2 -L 1 .

[0050] The compensation length of the driving rope is: the difference between the spatial lengths of the driving rope before and after rotating a preset angle on the input shaft, that is, L 2 -L 1 . The purpose expected to be achieved by the compensation relationship provided in the embodiment of the present application is: during the process that the spatial length of the driving rope changes from L 1 to L 2 , by compensating for the elongation amount of the driving rope caused by the input shaft and making the elongation amount of the traction wire be R 0 *dθ, so that the change amount of the traction wire and the rotation angle of the input shaft satisfy a linear relationship. To achieve this purpose, during the process that the spatial length of the driving rope changes from L 1 to L2 During the process, there should be a change in the spatial length L 2 -L 1 and the stretching amount R of the designed traction wire 0 *dθ sum up to the total stretching length dl of the input shaft, that is: dl = L 2 -L 1 +R 0 *dθ, so that the total stretching length dl of the input shaft compensates for the change in the spatial length L of the drive rope 2 -L 1 , and the stretching amount of the designed traction wire is R 0 *dθ.

[0051] Step S13: Determine the functional relationship between the radius of the input shaft and the rotation angle of the input shaft according to the compensation relationship.

[0052] The following expands and explains the specific implementation method of step S13.

[0053] Combined with Figure 1 and Figure 3 shown, respectively obtain the vertical distances between the input shaft and the traction wire at different positions;

[0054] In some embodiments, the L 1 satisfies: wherein, the R 1 is the radius of the input shaft corresponding to the spatial length L of the drive rope 1 , the P is the pitch of the input shaft, and the Pθ / 2π is the lead corresponding to the input shaft rotating by θ angle.

[0055] Figure 2 is Figure 1 a partial schematic diagram.

[0056] See Figure 1 and Figure 2 , through the spatial geometric relationship, the lengths of the three right-angled sides of the right-angled triangle are calculated respectively by the vertical distance, L 1 and Pθ / 2π. L 1 is used as the length of the hypotenuse of the right-angled triangle, and the lengths of the other two right-angled sides are respectively R 0 -R 1 +Y and Pθ / 2π. The side with a length of R 0 -R 1 +Y in the vertical direction of the right-angled side is L 1The vertical distance in the 0 state, where the side with length Pθ is the horizontal right-angled side. Among them, after rotating by an angle θ from the initial state (the state where the rotation angle is zero, which can be understood as the position when the driving rope has not started to rotate or when the driving rope is at the end of the input shaft), the vertical distance also increases accordingly. It can be understood that the radius of the input shaft changes from R 1 to R 0 -R 1 +Y. That is to say, the increase / decrease amount of the vertical distance is the decrease / increase amount of the radius, and the changed vertical distance is the length of the vertical right-angled side. Among them, Pθ / 2π is the lead of the input shaft after rotating by θ, which is the side length of the horizontal right-angled side. Finally, according to the relationship between the side lengths of the right triangle, formula (2) can be obtained. It should be noted that Figure 2 For the input shaft with a variable radius structure in

[0057] , from left to right in the figure, the radius gradually decreases. Of course, in actual applications, an input shaft with a gradually increasing radius from left to right can also be set. 2 In some embodiments, the L satisfies: 2 where the R 2 is the radius of the input shaft corresponding to the driving rope with a spatial length of L

[0058] See Figure 1 . Similarly, according to the geometric relationship of the spatial triangle, the vertical distance, L 2 and P(θ + dθ) / 2π are the side lengths of the three sides of the right triangle respectively. L 2 is used as the length of the hypotenuse of the right triangle, and the lengths of the other two right-angled sides are R 0 -R 2 +Y and P(θ + dθ) / 2π. The side with length R 0 -R 2 +Y is the vertical right-angled side, that is, the vertical distance in the 2 state, and the side with length P(θ + dθ) / 2π is the horizontal right-angled side. Among them, see Figure 2 . After rotating by an angle (θ + dθ) from the initial state (such as the state when not rotating), the vertical distance also increases accordingly. It can be understood that whether the radius of the input shaft is getting larger or smaller from R 0 to R 2 , the vertical distance changes from the initial Y to R 0 -R 2+Y, the changed vertical distance is the length of the vertical right-angled side. That is to say, the change amount of the vertical distance is the change amount of the radius. At the same time, the side with a length of P(θ + dθ) / 2π in the horizontal direction is the lead of the input shaft after the input shaft rotates by θ + dθ. Finally, according to the relationship between the side lengths of the right-angled triangle, formula (3) can be obtained.

[0059] In some embodiments, the dl satisfies:

[0060] Specifically, formula (4) is obtained according to the cosine theorem of a right-angled triangle. Refer to Figure 3 , in the right-angled triangle AEF, AF is the right-angled side of the right-angled triangle AFC (the lengths of the other two sides are Pdθ / 2π and dl respectively, where Pdθ / 2π is the side length of the right-angled side and dl is the side length of the hypotenuse). Figure 3 In, the rotation axis is the central axis of the radius input shaft. That is to say, during the rotation of the input shaft by the driving rope, it rotates around the rotation axis. It can be understood that dl represents the length of the hypotenuse AC in Figure 4 , where L 1 the radius R in the state of 1 will rotate from EB to EA, and when in the state of EA, the input shaft has rotated by an angle of θ + dθ. At this time, the length of the radius CD is R 2 , and point B moves from the input shaft radius from R 1 to R 2 During the transformation process, the actual movement trajectory is from point B to point A, but the actual final tangent point is C. Then dl represents the total length by which the input shaft is stretched during the rotation process. Finally, according to the cosine theorem, the above formula (4) can be obtained.

[0061] In some embodiments, determining the functional relationship between the radius of the input shaft and the rotation angle of the input shaft according to the compensation relationship specifically includes:

[0062] Substitute formulas (1)-(3) into (4) to obtain the functional relationship:

[0063]

[0064] Specifically, formula (4) is the cosine theorem of a triangle, corresponding to Figure 3 the right-angled sides AB, BC and the hypotenuse AC in. And substitute the respective equality relationships of formulas (1)-(3) into formula (4) respectively, and the above functional relationship between the radius and the rotation angle can be obtained.

[0065] Among them, R 1 is the radius of the input shaft after the input shaft drives the driving rope to rotate by an angle of θ, and R 2is the radius of the input shaft after the input shaft drives the drive rope to rotate by an angle of θ + dθ. Therefore, let R 1 = R(θ), R 2 = R(θ + dθ), where R 1 = R(θ) represents the linear function between the radius R 1 and the rotation angle θ, and R 2 = R(θ + dθ) represents the linear function between the radius R 2 and the rotation angle θ + dθ.

[0066] Step S14: Determine the radius of the input shaft according to the function relationship.

[0067] Determine the radius of the input shaft according to the function relationship. Specifically, substitute multiple sets of preset angle values into the function relationship to obtain multiple sets of radii of the input shaft.

[0068] Specifically, first determine multiple sets of preset angle values, and then substitute the multiple sets of preset angle values into the above function relationship, and the function relationship between the radii of the input shaft corresponding to the multiple sets of preset angle values can be obtained. It can be understood that the preset angle value is a value range.

[0069] Exemplarily, the preset angle value takes values between 30° and 90°. Among them, the more groups of preset angle values, the more corresponding radius quantities are obtained, which is beneficial to improving the production accuracy of the input shaft. However, the number of groups of preset angles should not be too many. Since the determination of the radius of the input shaft is also related to the constant pitch, on the premise of ensuring the winding effect of the drive rope, reasonably set the number of groups of preset angles. At the same time, during the production process, the values and the number of groups of preset angles can be adaptively adjusted according to the actual production accuracy.

[0070] Transmit the function relationship between the radii of the input shaft corresponding to multiple sets of preset angle values to the control program of the numerical control machine tool. When the lathe tool advances axially at a constant pitch P, adjust the radial change amount to form a changing radius, and thus an input shaft with a variable radius structure can be produced.

[0071] The method for determining the variable-radius input shaft provided by the embodiment of the present application can obtain a variable-radius input shaft, so that the traction wire changes linearly with the rotation angle of the input shaft, which is convenient for the control software to implement and improve the control accuracy.

[0072] An embodiment of the present application further provides a control method for a flexible arm robot, which is applied to a continuum flexible arm robot. The flexible arm robot includes the input shaft, the traction wire, the drive rope, and the control unit. The drive rope is wound around the input shaft. One end of the drive rope away from the input shaft is connected to the traction wire, and the input shaft is a columnar structure with a variable radius. The radius of the input shaft is determined according to the radius determination method in any of the above embodiments.

[0073] Figure 5 It is a structural schematic diagram of the flexible arm robot in the embodiment of the application.

[0074] See Figure 5 , the motor group controls the rotation of the input shaft. A drive rope is wound around the input shaft. The drive rope is connected to the proximal continuum of the continuum flexible arm robot. One end of the proximal continuum away from the drive rope is the distal continuum. Among them, the tungsten wire rope is the drive rope in the present application.

[0075] Figure 6 It is a schematic flowchart of the control method of the flexible robot in the embodiment of the present application.

[0076] See 6, the control method includes:

[0077] Step S21: Obtain the joint space angle of the flexible arm robot;

[0078] Specifically, the joint space angle of the continuum flexible arm robot can be obtained through actual measurement.

[0079] Exemplarily, the joint space angle of the flexible arm robot can be understood as the angle of the central axis where the joint of the flexible arm robot is located in the space coordinate system (x, y, z) in the space coordinate system.

[0080] Step S22: Calculate the joint space change amount of the flexible arm robot according to the space angle;

[0081] According to the angle of the joint, the continuum joint space change amount can be calculated, so as to obtain the change amount of the joint of the continuum flexible arm robot in the space after moving for a certain time. Subsequently, the accuracy of the joint position positioning is determined through this change amount.

[0082] Exemplarily, the joint space change amount can be understood as the difference between the joint space positions or space angles before and after the change.

[0083] For example, at the first movement moment, the spatial joint is at the first position (x1, y1, z1). From the first movement moment to the second movement moment, the spatial joint is at the second position (x2, y2, z2). Then the spatial change amount of the joint is the spatial distance from the first position (x1, y1, z1) to the second position (x2, y2, z2).

[0084] Also for example, at the first movement moment, the first spatial angle of the spatial joint is represented by the vector (a1, b1, c1). From the first movement moment to the second movement moment, the second spatial angle of the spatial joint is (a2, b2, c2). Then the change amount of the joint space is the angular difference from the first spatial angle (a1, b1, c1) to the second spatial angle (a2, b2, c2).

[0085] It should be noted that the above examples are only exemplary explanations of the spatial change amount of the joint space, and are not specifically limited.

[0086] Step S23: Determine the current position information of the drive rope according to the spatial change amount;

[0087] Specifically, the current position of the drive rope is determined by the spatial change amount of the flexible arm joint. It can be understood that the drive rope drives the joint of the flexible arm through the traction wire. Therefore, judging the current position of the drive rope indicates whether the continuum flexible arm robot has been controlled in place at this time.

[0088] Exemplarily, the current position information may include the spatial coordinate position and the spatial angle.

[0089] Step S24: Determine the preset position information of the drive rope according to the functional relationship between the radius of the input shaft and the rotation angle of the input shaft;

[0090] According to the functional relationship between the input shaft and the rotation angle of the input shaft, the preset position information of the drive rope is determined. At this time, the preset position information represents the position where the drive rope should be in the space in the current state.

[0091] Step S25: Update the current position of the drive rope according to the preset position information and the current position information.

[0092] Specifically, according to the relationship between the preset position information and the current position information, if it is within the given range, it is considered that the movement is in place, and the current position of the drive rope is updated, that is, the joint angle of the continuum flexible arm robot is updated, and this step is exited to start the control of the next cycle.

[0093] Exemplarily, the preset position information can be understood as the position where the flexible arm robot is currently located or should be located at the current moment. It can be understood that since the flexible arm robot is applied in minimally invasive surgery, controlling the accuracy of the flexible arm robot is particularly important. At each moment of movement, the flexible arm robot has a preset position, and this is used to indicate whether the joint has moved into place at this moment. Thus, the current position information is updated based on the preset position information to achieve high-precision control of the joints of the flexible robot.

[0094] Figure 7 This is a flowchart of the method for obtaining preset position information in the embodiments of the present application.

[0095] Referring to FIG. 7, in some embodiments, step S24, that is, determining the preset position information of the driving rope according to the functional relationship between the radius of the input shaft and the rotation angle of the input shaft, can be specifically implemented through the following steps S31 - S32:

[0096] Step S31: Obtain the current rotation angle of the input shaft;

[0097] Step S32: Substitute the rotation angle into the functional relationship to determine the preset position information of the driving rope.

[0098] Based on the rotation angle of the input shaft, the position information where the driving rope should currently be located is determined, which is the preset position information.

[0099] Figure 8 This is a flowchart of the method for updating the current position of the driving rope in the embodiments of the present application.

[0100] Referring to FIG. 8, in some embodiments, step S25, that is, updating the current position of the driven rope according to the preset position information and the current position information, can be specifically implemented through the following steps S41 - S44:

[0101] Step S41: Calculate the error between the current position information and the preset position information;

[0102] Step S42: Determine whether the error reaches the positioning accuracy; if so, execute step S43; if not, execute step S44;

[0103] Exemplarily, the positioning accuracy can be understood as the allowable range of the error. If the calculated error is within the allowable range, it indicates high positioning accuracy.

[0104] Exemplarily, the positioning accuracy can be understood as the magnitude relationship between the error and the actual preset error. For example, it is represented by a percentage. The smaller the percentage, the smaller the error and the higher the positioning accuracy.

[0105] Of course, the error representing the positioning accuracy described above is only exemplary and is not specifically limited.

[0106] Step S43: Update the current position;

[0107] Step S44: Determine whether the calculation times of the error have exceeded a preset number of times; if so, execute Step S43; if not, return to execute Step S22.

[0108] Specifically, calculate the error between the current position information and the preset position information. The error includes the error of the spatial coordinate position and the error of the rotation angle of the input shaft. And when the error meets the positioning accuracy, update the current position. When it does not meet the accuracy, determine whether the number of times of calculating the error has exceeded the preset number of times. If it has not exceeded the preset number of times, update the current position. If it has exceeded the preset number of times, return to execute the step of calculating the change amount of the continuum joint space according to the spatial angle.

[0109] Figure 9 It is a schematic diagram of the control flow of the control method in a feasible embodiment of the present application.

[0110] Referring to FIG. 9, in a specific embodiment, the input shaft of the present application is a winding column, the traction wire is an alloy wire, and the drive rope is a flexible tungsten wire rope. The control flow includes:

[0111] Step S51: Obtain the current spatial joint angle; the next step is to execute S54.

[0112] Step S52: Determine the radius of the winding column according to the input shaft variable radius compensation scheme; the next step is to execute S57.

[0113] Step S53: Given the target position of the distal continuum; the next step is to execute S58.

[0114] Step S54: Calculate the change amount of the joint space velocity through the Jacobian matrix.

[0115] Step S55: Calculate the change amount of the distal continuum joint space.

[0116] Specifically, the flexible body robot includes a distal continuum and a proximal continuum. Among them, one end of the proximal continuum joint is connected to the traction wire, and the other end is connected to the distal continuum.

[0117] Exemplarily, the change amount of the distal continuum joint space refers to the change amount of the axis or end of the distal continuum joint in space before and after movement, where the change amount can be represented by distance or angle parameters. The same is true for the change amount of the proximal continuum joint space.

[0118] Step S56: Calculate the change amount of the proximal continuum joint space.

[0119] Step S57: Update the spatial position after the tungsten wire rope is driven.

[0120] Step S58: Calculate the position and angle errors in the workspace.

[0121] Herein, the workspace refers to the spatial position where the joints of the flexible-arm robot are located, and can also be understood as the allowable range of movement of the joints of the flexible-body robot. Exemplarily, if the flexible-arm robot exceeds the established range of movement, it will seriously affect the program being executed.

[0122] Step S59: Determine whether the positioning accuracy is achieved; if yes, execute Step S60; if not, execute Step S61.

[0123] Exemplarily, the positioning accuracy can be understood as the allowable range of errors. If the calculated error is within the allowable range, it indicates high positioning accuracy.

[0124] Exemplarily, the positioning accuracy can be understood as the magnitude relationship between the error and the actual preset error. For example, expressed as a percentage, the smaller the percentage, the smaller the error and the higher the positioning accuracy.

[0125] Of course, the errors characterizing the positioning accuracy above are only exemplary and are not specifically limited.

[0126] Step S60: Update the joint-space angle.

[0127] Step S61: Determine whether the calculation exceeds the number of loops; if yes, execute Step S60; if not, execute Step S62.

[0128] Step S62: Calculate the adjustment amounts of the workspace velocity and angular velocity.

[0129] Step S63: Start the next target pose.

[0130] It can be understood that, referring to Figure 9 , there are three control starting points in the control flow diagram, including: Step S51, Step S52, and Step S53. Herein, Step S54 is executed after Step S51, Step S57 is executed after Step S52, and Step S58 is executed after Step S53.

[0131] Specifically, referring to Figure 9 , usually the remote master hand remotely gives the distal continuum target pose [p t R t . Comparing this pose with the current pose [p R] of the distal continuum flexible arm, the position and angle errors in the workspace can be determined:

[0132] The control objective of this step is to make the error reach the given range. Next, judge this error. If it is within the given range, it is considered that the movement is in place, update the joint angle, exit this step, and start the control of the next target pose. Among them, the pose represents the position and attitude. p t represents the target position, R t represents the target angle, p represents the current position, and R represents the current angle.

[0133] If the error does not reach the accuracy and the calculation times do not exceed the upper limit, continue the control of this target pose. Among them, the upper limit times is the maximum calculation times for the continuum flexible arm to reach each target pose. If it exceeds, it is considered that the movement to the target position fails and exits. For example, there are some special reasons that make the control not in place.

[0134] Next, first calculate the required linear velocity and angular velocity [v w] according to the error. The angular velocity in this workspace is used to calculate the change in joint space velocity dq through the inverse Jacobian matrix. Further, the joint pose increment of the new control period can be calculated by integration. According to actual requirements, there are two cases: linear velocity v priority and angular velocity w priority. Specifically, according to the actual control working conditions, in some working conditions, it is necessary to reach the position first, and linear velocity priority is selected; in some working conditions, it is necessary to adjust the attitude in place first, and angular velocity priority is selected.

[0135] For example, the linear velocity and angular velocity in the workspace are calculated by the control algorithm according to the position and angle errors in the workspace, such as the PID (proportional, integral, derivative) algorithm. The linear velocity and angular velocity in the workspace are transformed into the joint space according to the following two formulas. The calculation method of calculating dq according to [v w] is as follows: Calculate the Jacobian relationship between the joint space velocity and the workspace velocity:

[0136] ① Calculate the Jacobian relationship between the joint space velocity and the workspace velocity:

[0137]

[0138] ② Calculate the joint space velocity according to linear velocity priority or angular velocity priority:

[0139] Linear velocity priority:

[0140] Angular velocity priority:

[0141] Among them: J * 、H * are the inverse matrices of J and H, and I is the identity matrix. is the joint space velocity

[0142] ③ Calculate the joint position increment of the new period: T is the control period. Among them, the calculation period is determined according to the actual computing performance of the controller. The stronger the computing power, the smaller the calculation period can be. Specifically, the value of the control period is adaptively adjusted according to the actual computing power.

[0143] Among them, the joints of the distal continuum can be mapped to obtain the change in the joint space velocity dq of the proximal continuum by equal ratio (here 1:1 is taken, but not limited to 1:1). During the process of the drive rope driving the proximal end of the input shaft, the change in the length of the space segment between the input shaft and the traction wire caused by the rotation of the input shaft can be compensated by the input shaft with a variable radius, so that the change in the length of the traction wire and the rotation angle of the input shaft satisfy a linear relationship.

[0144] After the drive rope drives, the updated working space position will be recalculated, and then the error will be calculated and enter the next cycle until the set target pose is reached or the number of cycles is exceeded and it exits.

[0145] The present application also provides a flexible arm robot, including an input shaft, a traction wire, a drive rope, and a control unit. The drive rope is wound around the input shaft. One end of the drive rope away from the input shaft is connected to the traction wire, and the input shaft is a columnar structure with a variable radius; the radius of the input shaft is determined by the radius determination method provided in the above embodiment, and the control unit is used to control the rotation of the input shaft and execute the control method introduced in the above embodiment. The flexible arm robot provided by the embodiment of the present application includes all the beneficial technical effects of the radius determination method and the flexible arm robot control method provided in the above embodiment, and will not be elaborated here.

[0146] For the similarities between the embodiments provided in the present application, reference can be made to each other. The specific embodiments provided above are only several examples under the general concept of the present application and do not constitute a limitation on the protection scope of the present application. For those skilled in the art, any other implementation manner extended based on the solution of the present application without creative efforts belongs to the protection scope of the present application.

Claims

1. A method for determining the radius of an input shaft, wherein the input shaft drives a flexible arm to move. The flexible arm robot includes the input shaft, a traction wire, a driving rope, and the flexible arm. The driving rope is wound around the input shaft and drives the flexible arm to move. One end of the driving rope away from the input shaft is connected to the traction wire, and the input shaft is a columnar structure with a variable radius. Characterized in that: The method includes: Determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft; Determining a compensation relationship according to the change amount of the traction wire, the compensation length of the driving rope, and the change amount of the driving rope; wherein, the compensation length is the change amount of the spatial length of the driving rope; Determining the functional relationship between the radius of the input shaft and the rotation angle of the input shaft according to the compensation relationship; the compensation relationship is: the change amount of the driving rope is equal to the sum of the change amount of the traction wire and the compensation length of the driving rope; the functional relationship is: Among them, θ and dθ are rotation angles, and R 0 is the cross-sectional radius of the input shaft corresponding to the starting point where the driving rope winds around the input shaft; P is the pitch of the input shaft; Y is the distance between the traction wire and the vertical direction of the end of the input shaft, Y is the initial vertical distance, R(θ) is a linear function between the radius of the input shaft after the input shaft drives the driving rope to rotate by an angle θ and the rotation angle θ, and R(θ + dθ) is a linear function between the radius of the input shaft after the input shaft drives the driving rope to rotate by an angle θ + dθ and the rotation angle θ + dθ; Determining the radius of the input shaft according to the functional relationship.

2. The radius determination method according to claim 1, Characterized in that: The determination of the change amount of the traction wire according to the change amount of the rotation angle of the input shaft is: the change amount of the traction wire is proportional to the rotation angle of the input shaft.

3. The radius determination method according to claim 1, Characterized in that: The compensation length of the driving rope is: the difference between the spatial lengths of the driving rope before and after rotating a preset angle on the input shaft.

4. The radius determination method according to claim 1, Characterized in that: The determination of the change amount of the traction wire according to the change amount of the rotation angle of the input shaft is: determining the change amount of the traction wire according to the change amount of the rotation angle of the input shaft and the cross-sectional radius of the input shaft corresponding to the starting point where the driving rope is wound around the input shaft.

5. A control method for a flexible arm robot, applied to a flexible arm robot. The flexible arm robot includes the input shaft, a traction wire, a driving rope, and a control unit. The driving rope is wound around the input shaft. One end of the driving rope away from the input shaft is connected to the traction wire, and the input shaft is a columnar structure with a variable radius. The radius of the input shaft is determined according to the radius determination method of any one of claims 1-4. Characterized in that: The control method includes: Obtaining the joint space angle of the flexible arm robot; Calculating the joint space change amount of the flexible arm robot according to the space angle; Determining the current position information of the driving rope according to the space change amount; Determining the preset position information of the driving rope according to the functional relationship between the radius of the input shaft and the rotation angle of the input shaft; Updating the current position of the driving rope according to the preset position information and the current position information.

6. The control method according to claim 5, Characterized in that: The determination of the preset position information of the driving rope according to the functional relationship between the radius of the input shaft and the rotation angle of the input shaft specifically includes: Obtaining the current rotation angle of the input shaft; Substitute the rotation angle into the functional relationship to determine the preset position information of the drive rope.

7. The control method according to claim 6, wherein, the updating of the current position of the drive rope according to the preset position information and the current position information specifically includes: calculating the error between the current position information and the preset position information, the error including the coordinate position error and the rotation angle error; judging whether the error reaches the positioning accuracy; if so, updating the current position; if not, judging whether the number of calculations of the error has exceeded the preset number of times; and updating the current position when the number of calculations of the error exceeds the preset number of times.

8. The control method according to claim 7, wherein, the method further includes: if the number of calculations of the error does not exceed the preset number of times, calculating the joint space change amount of the flexible arm robot according to the spatial angle.

9. A flexible arm robot, wherein, comprising an input shaft, a traction wire, a drive rope and a control unit, the drive rope is wound around the input shaft, one end of the drive rope away from the input shaft is connected to the traction wire, and the input shaft is a columnar structure with a variable radius; wherein, the radius of the input shaft is determined according to the radius determination method described in any one of claims 1-4; the control unit is used to control the rotation of the input shaft and execute the control method described in any one of claims 5-8.

Images