A vision-based method for detecting the end position of a rope-driven continuum robot

Abstract

The present invention relates to a vision-based method for detecting the end position of a rope-driven continuum robot. The method obtains a top detection point of a ball at the end of the rope-driven continuum robot along the robot's central axis, as well as three detection points set on a circular disc base, defined as first, second, third, and fourth target detection points. Using a binocular camera, the robot's left and right eye images are captured, respectively. Based on the left and right eye images, the three-dimensional position coordinates of the four target detection points in the binocular camera coordinate system are obtained. Based on the obtained three-dimensional position coordinates, the robot's bending angle and rotation angle are determined using stereo geometry methods. This method does not require complex algorithms or a transformation matrix between the robot's end coordinate system and its base coordinate system. Therefore, the method is simple and easy to implement, ensuring detection accuracy while also saving costs.

Description

Technical Field

[0001] The present invention relates to the technical field of machine vision detection, and in particular to a method for detecting the end position of a rope-driven continuum robot based on vision. Background Art

[0002] Traditional rigid-body robots, made of hard materials, offer advantages such as high precision and strong load capacity. However, their limited freedom of movement and flexibility make them difficult to adapt to confined spaces. Unlike rigid-body robots, tether-driven continuum robots, as biomimetic robots, offer superior compliance, flexibility, and safety, making them better able to adapt to unstructured environments. Consequently, they are attracting increasing attention and research.

[0003] In order to better apply rope-driven continuum robots, they need to be precisely controlled. To achieve this, the end position of the continuum robot must be detected. Related technologies currently use magnetic positioning and visual detection methods to detect the end position of continuum robots. However, the magnetic positioning method is relatively expensive, while existing visual detection methods require the transformation matrix between the robot's end coordinate system and the base coordinate system to be known. This transformation matrix is ​​difficult to obtain for continuum robots. Furthermore, most visual detection methods focus on detecting the shape of continuum robots, and their detection algorithms are relatively complex. Summary of the Invention

[0004] In order to solve the above technical problems, the present invention provides a vision-based end position detection method for a rope-driven continuum robot.

[0005] A vision-based method for detecting the end position of a rope-driven continuum robot, comprising:

[0006] Obtain the top detection point of the end ball of the rope-driven continuum robot along the robot's central axis, as well as three detection points set on the disc base, which are defined as the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point. The end ball is connected to the end support disc of the robot body via a connecting device, so that the top detection point during the robot's movement can be more accurately detected by the binocular camera;

[0007] Based on the binocular camera, a left-eye image and a right-eye image of the rope-driven continuum robot are respectively acquired, and based on the left-eye image and the right-eye image, the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system are acquired;

[0008] According to the acquired three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point and the fourth target detection point in the binocular camera coordinate system, the bending angle and the rotation angle of the rope-driven continuum robot are obtained by a stereo geometry method.

[0009] In one embodiment, the three detection points provided on the disc base are three adjacent hexagonal screw positions among six hexagonal screw positions equally distributed along the circumference of the disc base.

[0010] In one embodiment, obtaining the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system according to the left-eye image and the right-eye image includes:

[0011] Performing distortion correction on the left-eye image and the right-eye image according to the intrinsic and extrinsic parameter matrices and the distortion coefficients of the binocular camera;

[0012] Obtaining, based on the distortion-corrected left-eye image and the right-eye image, connected domains corresponding to the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point, respectively, and obtaining the two-dimensional position coordinates of the center point of each connected domain;

[0013] According to the two-dimensional position coordinates of each center point, the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point and the fourth target detection point in the binocular camera coordinate system are obtained.

[0014] In one embodiment, obtaining the two-dimensional position coordinates of the center point of each connected region includes:

[0015] Acquire, according to each connected domain, a region boundary attribute corresponding to each connected domain, wherein the region boundary attribute is a two-dimensional position coordinate of a region boundary corresponding to the connected domain;

[0016] The two-dimensional position coordinates of the center point of each connected region are obtained according to the region boundary attributes.

[0017] In one embodiment, the intrinsic and extrinsic parameter matrices and the distortion coefficients of the binocular camera are obtained by calibrating the binocular camera using a MATLAB toolbox.

[0018] In one embodiment, obtaining the bending angle and the rotation angle of the rope-driven continuum robot by a stereo geometry method based on the acquired three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system includes:

[0019] Obtaining the three-dimensional position coordinates of a first intermediate point, a second intermediate point, a third intermediate point, a fourth intermediate point, a fifth intermediate point, and a sixth intermediate point in the binocular camera coordinate system according to the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system; wherein the first intermediate point, the second intermediate point, the third intermediate point, the fourth intermediate point, and the fifth intermediate point are located on a disk base, the disk base is arranged above and parallel to the disk base, the first intermediate point is the center point of the disk base, a line between the first intermediate point and the center point of the disk base, a line between the second intermediate point and the second target detection point, and a line between the third intermediate point and the fourth target detection point are all perpendicular to the plane of the disk base, the fourth intermediate point is on the reverse extension line of the line between the first intermediate point and the third intermediate point, the fifth intermediate point is the projection of the first target detection point on the plane of the disk base, and the sixth intermediate point is the midpoint between the first intermediate point and the first target detection point;

[0020] Calculate the bending angle of the rope-driven continuum robot using the law of cosines based on the three-dimensional position coordinates of the first, fifth, and sixth intermediate points in the binocular camera coordinate system; the bending angle is the angle formed by the intersection of a plane direction including the center of the terminal sphere and perpendicular to the central axis of the robot and the line connecting the first and fifth intermediate points;

[0021] Determine whether there is an intersection between the line segment formed by connecting the second intermediate point and the fifth intermediate point and the line segment formed by connecting the third intermediate point and the fourth intermediate point. If so, calculate the rotation angle of the rope-driven continuum robot based on the three-dimensional position coordinates of the first intermediate point, the third intermediate point and the fifth intermediate point in the binocular camera coordinate system and the cosine theorem; if not, calculate the mutual angle of the rotation angle of the rope-driven continuum robot based on the three-dimensional position coordinates of the first intermediate point, the third intermediate point and the fifth intermediate point in the binocular camera coordinate system and the cosine theorem; the rotation angle is the angle formed by the intersection of the line connecting the first intermediate point and the third intermediate point and the line connecting the first intermediate point and the fifth intermediate point.

[0022] The beneficial effects of the present invention include: based on the coordinates of the detected finite target point in the binocular camera coordinate system, the method can directly obtain the end position of the rope-driven continuum robot in the world coordinate system through the stereo geometry method, without the need for complex algorithms and the conversion matrix between the end coordinate system of the rope-driven continuum robot and the base coordinate system. Therefore, the method is simple and easy to implement, and can save costs while ensuring detection accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments:

[0024] Figure 1 This is a flow chart of a method for detecting the end position of a rope-driven continuum robot based on vision provided in an embodiment of the present application;

[0025] Figure 2 Schematic diagram of a continuum structure of a method for detecting the end position of a rope-driven continuum robot based on vision provided in an embodiment of the present application;

[0026] Figure 3 This is a schematic diagram of the base disc structure of a vision-based end position detection method of a rope-driven continuum robot provided in an embodiment of the present application. DETAILED DESCRIPTION

[0027] In the following description, specific details such as specific system structures and techniques are provided for purposes of illustration rather than limitation to facilitate a thorough understanding of the embodiments of the present application. However, it will be apparent to those skilled in the art that the present application may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid obscuring the description of the present application with unnecessary detail.

[0028] It should be understood that when used in the present specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or collections thereof.

[0029] It will also be understood that the term "and / or" used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.

[0030] As used in this specification and the appended claims, the term "if" can be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrase "if it is determined" or "if [described condition or event] is detected" can be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

[0031] In addition, in the description of the present application specification and the appended claims, the terms "first", "second", "third", etc. are only used to distinguish the descriptions and cannot be understood as indicating or implying relative importance.

[0032] References to "one embodiment" or "some embodiments" in this specification mean that a particular feature, structure, or characteristic described in conjunction with that embodiment is included in one or more embodiments of the present application. Thus, phrases such as "in one embodiment," "in some embodiments," "in other embodiments," and "in other embodiments" appearing in various places in this specification do not necessarily refer to the same embodiment, but rather mean "one or more but not all embodiments," unless otherwise specifically emphasized. The terms "including," "comprising," "having," and variations thereof all mean "including but not limited to," unless otherwise specifically emphasized.

[0033] In order to illustrate the technical solution described in this application, a specific implementation method is provided below.

[0034] like Figure 1 As shown, the embodiment of the present application provides a vision-based method for detecting the end position of a rope-driven continuum robot, comprising the following steps:

[0035] Step S1: Obtain the top detection point of the end ball of the rope-driven continuum robot along the robot's central axis, as well as three detection points set on the disc base, which are defined as the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point:

[0036] Obtain the ball detection point at the end of the rope-driven continuum robot and the three detection points set on the disc base, which are defined as the first target detection point, the second target detection point, the third target detection point and the fourth target detection point. As a specific implementation method, Figure 2 and Figure 3 As shown, it should be noted that the end ball detection point is the topmost point of the ball 03 along the robot center axis 04. The end ball is connected to the end support plate 05 of the robot body 1 through the connection device 04, so that the top detection point of the robot during movement can be detected more accurately by the binocular camera, that is, the first target detection point. The first target detection point is Figure 2 The first target detection point T in the figure, wherein the connecting device 04 is a metal wire. The three detection points set on the disc base 01 are three adjacent hexagonal screw positions among the six hexagonal screw positions equally distributed along the circumference of the disc base 01, namely the second target detection point A, the third target detection point B, and the fourth target detection point C. Among them, the end ball 03 of the rope-driven continuum robot and the three detection points set on the disc base 01 are marked in black, red, green, and blue, respectively. It can be understood that during visual inspection, the image feature recognition effect can be enhanced by marking the target points with different colors, thereby improving the detection accuracy.

[0037] Step S2: Based on the binocular camera, the left eye image and the right eye image of the rope-driven continuum robot are respectively obtained, and the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system are obtained based on the left eye image and the right eye image:

[0038] Using a binocular camera, the left-eye image and right-eye image of the rope-driven continuum robot are respectively acquired. Based on the left-eye image and the right-eye image, the three-dimensional position coordinates of the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C in the binocular camera coordinate system are acquired. As a specific embodiment, a specific process for acquiring the three-dimensional position coordinates of the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C in the binocular camera coordinate system is given below:

[0039] According to the intrinsic and extrinsic parameter matrices and distortion coefficients of the binocular camera, the left eye image and the right eye image are subjected to distortion correction. In this embodiment, the intrinsic and extrinsic parameter matrices and distortion coefficients of the binocular camera are obtained by calibrating the binocular camera using the MATLAB toolbox.

[0040] Based on the distortion-corrected left-eye image and right-eye image, connected domains corresponding to the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C are obtained, and the two-dimensional position coordinates of the center points of each connected domain are obtained. Specifically, the connected domain may be the maximally connected domain that includes the corresponding target detection point. Based on the distortion-corrected left-eye image and right-eye image, connected domains corresponding to the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C are obtained, specifically by first obtaining initial position information of the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C in the left-eye image and the right-eye image after distortion correction. Then, based on the initial position information of the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C in the left-eye image and the right-eye image, connected domains corresponding to the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C in the left-eye image and the right-eye image are obtained. Next, the two-dimensional position coordinates of the center point of each connected domain are obtained. As a specific implementation, the region boundary attributes corresponding to each connected domain are first obtained based on the connected domain. Specifically, the region boundary attributes corresponding to the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C are obtained. The region boundary attributes are the two-dimensional position coordinates of the region boundary. The two-dimensional position coordinates of the region boundary can be the two-dimensional position coordinates of each edge pixel point in the region boundary. Based on the two-dimensional position coordinates of each edge pixel point in the region boundary, the two-dimensional position coordinates of the center point of each connected domain are obtained.

[0041] According to the two-dimensional position coordinates of the center point of each connected domain, the three-dimensional position coordinates of the first target detection point T, the second target detection point A, the third target detection point B and the fourth target detection point C in the binocular camera coordinate system are obtained respectively, which are expressed as (x T ,y T ,z T )、(x A ,y A ,z A )、(x B ,y B ,z B ) and (x C ,y C ,z C ).

[0042] Step S3: Based on the acquired three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system, the bending angle and the rotation angle of the rope-driven continuum robot are obtained by a stereo geometry method:

[0043] According to the three-dimensional position coordinates (x T ,y T ,z T )、(x A ,y A ,z A )、(x B ,y B ,z B ) and (x C ,y C ,z C ), the bending angle θ and rotation angle of the rope-driven continuum robot are obtained by solid geometry method As a specific implementation method, an implementation process is given below:

[0044] According to the three-dimensional position coordinates (x T ,y T ,z T )、(x A ,y A ,z A )、(x B ,y B ,z B ) and (x C ,y C ,z C), obtain the three-dimensional position coordinates of the first intermediate point O1, the second intermediate point A1, the third intermediate point C1, the fourth intermediate point D1, the fifth intermediate point E, and the sixth intermediate point F in the binocular camera coordinate system. The first intermediate point O1, the second intermediate point A1, the third intermediate point C1, the fourth intermediate point D1, and the fifth intermediate point E are located on the disk base 02. The disk base 02 is positioned above and parallel to the disk base 01. The connection method and distance between the disk base 02 and the disk base 01 are determined by actual conditions. The radius, thickness, material, and other parameters of the disk base 02 and the disk base 01 are also determined by actual conditions. The first intermediate point O1 is the center point of the disk base 02. The line connecting the first intermediate point O1 and the center point of the disk base 01, the line connecting the second intermediate point A1 and the second target detection point A, and the line connecting the third intermediate point C1 and the fourth target detection point C are all perpendicular to the plane of the disk base 02. The fourth intermediate point D1 is located on the reverse extension of the line connecting the first intermediate point O1 and the third intermediate point C1. The specific location of the fourth intermediate point D1 is set according to actual conditions. The fifth intermediate point E is the projection of the first target detection point T on the plane of the disk base O2. The sixth intermediate point F is the midpoint between the first intermediate point O1 and the first target detection point T.

[0045] According to the three-dimensional position coordinates of the first intermediate point O1, the fifth intermediate point E and the sixth intermediate point F in the binocular camera coordinate system, the bending angle θ of the rope-driven continuum robot is calculated by the cosine theorem; the bending angle θ is the angle formed by the intersection of the first direction and the second direction, wherein the first direction is the plane direction including the center of the end sphere and perpendicular to the central axis of the robot, and the second direction is the direction corresponding to the line connecting the first intermediate point O1 and the fifth intermediate point E. The bending angle θ is Figure 2 In ∠THE, the bending angle θ is in the range [0,π].

[0046] Determine whether there is an intersection between the line segment formed by the second intermediate point A1 and the fifth intermediate point E and the line segment formed by the third intermediate point C1 and the fourth intermediate point D1. If so, calculate the rotation angle of the rope-driven continuum robot based on the three-dimensional position coordinates of the first intermediate point O1, the third intermediate point C1, and the fifth intermediate point E in the binocular camera coordinate system and the law of cosines. If not, the rotation angle of the rope-driven continuum robot is calculated based on the three-dimensional position coordinates of the first intermediate point O1, the third intermediate point C1 and the fifth intermediate point E in the binocular camera coordinate system and the cosine theorem. The angle of rotation The angle formed by the intersection of the line connecting the first intermediate point O1 and the third intermediate point C1 and the line connecting the first intermediate point O1 and the fifth intermediate point E. The range is [0,2π].

[0047] Bending angle θ and rotation angle of the rope-driven continuum robot The specific algorithm process is as follows:

[0048] like Figure 3 As shown, through geometric analysis, we know that point M is the midpoint of line segment AC, so the coordinates of point M are Set O as the center point of the disk base 01. At the same time, point M is also the midpoint of line segment OB, so the coordinates of point O are (x O ,y O ,z O )=(x A +x C -x B ,y A +y C -y B ,z A +z C -z B ). Set the coordinates of the first intermediate point O1 to (x O1 ,y O1 ,z O1 ), the coordinates of the third intermediate point C1 are (x C1 ,y C1 ,z C1 ), the coordinates of the fifth intermediate point E are (x E ,y E ,z E ). Since the distance of OO1 is h = 11 mm, and Then we get the following equation:

[0049] (x O1 -x O ) 2 +(y O1 -y O ) 2 +(z O1 -z O ) 2 =h 2 (1)

[0050] (x O1 -x O )(x B -x A )+(y O1 -y O )(y B -y A )+(z O1 -z O )(z B -z A )=0 (2)

[0051] (xO1 -x O )(x B -x C )+(y O1 -y O )(y B -y C )+(z O1 -z O )(z B -z C )=0 (3)

[0052] Solving (2) and (3) together, we can obtain:

[0053] y O1 =a-bz O1 (4)

[0054] in,

[0055] Substituting (4) into (2) we can further obtain:

[0056] x O1 =c+dz O1 (5)

[0057] in,

[0058] Substituting (4) and (5) into (1) we can further obtain:

[0059]

[0060] Solving (6) we can get z O1 The value of z can be determined based on the Z coordinates of the first target detection point T, the second target detection point A, the third target detection point B, and the fourth target detection point C. O1 Which value of the two solutions is taken? That is, when the Z coordinate of the first target detection point T is greater than the Z coordinates of the second target detection point A, the third target detection point B, and the fourth target detection point C, z O1 Take the maximum value. When the Z coordinate of the first target detection point T is smaller than the Z coordinates of the second target detection point A, the third target detection point B, and the fourth target detection point C, z O1 Take the minimum value, and then get x according to (4) and (5) O1 and y O1 The value of , thereby obtaining the coordinates of the first intermediate point O1.

[0061] And O1C1∥OC and Right now

[0062]

[0063] Then the coordinates of the third middle point C1 are obtained. Similarly, the coordinates of the second middle point A1 and the fourth middle point D1 can be obtained.

[0064] In addition, since the sixth intermediate point F is the midpoint between the first intermediate point O1 and the first target detection point T, the coordinates of the sixth intermediate point F are

[0065] And because point E is on the plane where the disk base 02 is located, according to the normal vector of the plane where the disk base 02 is located The general equation of the plane where the disk base 02 is located can be written by using a point C1 in the plane where the disk base 02 is located:

[0066] (x O1 -x O )(xx C1 )+(y O1 -y O )(yy C1 )+(z O1 -z O )(zz C1 )=0 (8)

[0067] again Right now

[0068]

[0069] Combining equations (8) and (9), we can find m and then the coordinates of the fifth intermediate point E.

[0070] According to the three-dimensional position coordinates of the first intermediate point O1, the fifth intermediate point E and the sixth intermediate point F in the binocular camera coordinate system, the bending angle θ of the continuum robot is calculated by the cosine theorem.

[0071] like Figure 2 As shown in the figure, according to the constant curvature assumption, that is, the bending process of the rope-driven continuum robot is equivalent to a circular arc with constant curvature everywhere, we can get |TH| = |O1H|, and then through geometric analysis, we know that ∠O1FE = θ, so the value of the bending angle θ can be obtained according to the cosine theorem, that is:

[0072]

[0073] Determine whether there is an intersection between the line segment formed by the second intermediate point A1 and the fifth intermediate point E and the line segment formed by the third intermediate point C1 and the fourth intermediate point D1. If so, calculate the rotation angle of the continuum robot based on the three-dimensional position coordinates of the first intermediate point O1, the third intermediate point C1, and the fifth intermediate point E in the camera coordinate system and the law of cosines. If not, the rotation angle of the continuum robot is calculated based on the three-dimensional position coordinates of the first intermediate point O1, the third intermediate point C1 and the fifth intermediate point E in the camera coordinate system and the cosine theorem. It should be noted that the rotation angle The range is [0,2π].

[0074] like Figure 2 As shown, since the fifth intermediate point E is on both sides of C1D1, the angle calculated by applying the cosine theorem is When the size of is on the side of the second intermediate point A1, we need to subtract the angle calculated using the law of cosines from 2π. Therefore, we first need to determine which side of C1D1 the fifth intermediate point E is on. This is determined by whether line segments A1E and C1D1 intersect. That is, when line segments A1E and C1D1 intersect, the fifth intermediate point E is on the right side. If there is no intersection, the fifth intermediate point E is on the left side. Specifically:

[0075] The parametric equation of line segment A1E is:

[0076]

[0077] The parametric equation of line segment C1D1 is:

[0078]

[0079] By combining (11) and (12), we can find the values ​​of t1 and t2. The position of the fifth intermediate point E can be determined by judging whether their values ​​are in the range [0, 1]. When t1 and t2 are both in the range [0, 1], it means that line segment A1E intersects line segment C1D1, and the fifth intermediate point E is on the right. At this time:

[0080]

[0081] When at least one of t1 and t2 is not in the range [0, 1], it means that line segment A1E has no intersection with line segment C1D1, and the fifth middle point E is on the left. At this time:

[0082]

[0083] In summary, based on equations (10), (13) and (14), the bending angle θ and rotation angle of the rope-driven continuum robot configuration space can be obtained: Furthermore, according to the kinematics of the rope-driven continuum robot, the end position of the rope-driven continuum robot in the task space can be obtained.

[0084] Therefore, according to the vision-based end position detection method of the rope-driven continuum robot according to an embodiment of the present invention, the binocular cameras are calibrated using the MATLAB toolbox to obtain the intrinsic and extrinsic parameter matrices and distortion coefficients of the left and right cameras. Then, three adjacent hexagonal screws equally distributed along the circumference of the circle of the end ball and the disk base of the rope-driven continuum robot are marked black, red, green, and blue as the first, second, third, and fourth target detection points, respectively. The rope-driven continuum robot images corresponding to the left and right cameras are obtained respectively, and the left and right images are processed to obtain the three-dimensional position coordinates of the first, second, third, and fourth target detection points in the binocular camera coordinate system. Finally, based on the obtained three-dimensional position coordinates of the first, second, third, and fourth target detection points in the binocular camera coordinate system, the bending angle and rotation angle of the rope-driven continuum robot are obtained using a stereo geometry method. This method is based on the coordinates of the detected finite target point in the camera coordinate system. The end position of the rope-driven continuum robot in the world coordinate system can be obtained directly through solid geometry methods without the need for complex algorithms and the transformation matrix between the end coordinate system of the rope-driven continuum robot and the base coordinate system. Therefore, this method is simple and easy to implement, and can save costs while ensuring detection accuracy.

[0085] The above-described embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present application, and should all be included in the scope of protection of the present application.

Claims

1. A vision-based method for detecting the end position of a rope-driven continuum robot, characterized in that: include: Obtain the top detection point of the end ball of the rope-driven continuum robot along the robot's central axis, as well as three detection points set on the disc base, which are defined as the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point respectively; Based on the binocular camera, a left-eye image and a right-eye image of the rope-driven continuum robot are respectively acquired, and based on the left-eye image and the right-eye image, the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system are acquired; Obtaining a bending angle and a rotation angle of the rope-driven continuum robot by a stereo geometry method based on the acquired three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system; The step of obtaining the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system according to the left-eye image and the right-eye image includes: Performing distortion correction on the left-eye image and the right-eye image according to the intrinsic and extrinsic parameter matrices and the distortion coefficients of the binocular camera; Obtaining, based on the distortion-corrected left-eye image and the right-eye image, connected domains corresponding to the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point, respectively, and obtaining the two-dimensional position coordinates of the center point of each connected domain; According to the two-dimensional position coordinates of each center point, the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point and the fourth target detection point in the binocular camera coordinate system are obtained; The obtaining of the two-dimensional position coordinates of the center point of each connected region includes: Acquire, according to each connected domain, a region boundary attribute corresponding to each connected domain, wherein the region boundary attribute is a two-dimensional position coordinate of a region boundary corresponding to the connected domain; The two-dimensional position coordinates of the center point of each connected region are obtained according to the region boundary attributes.

2. The method for detecting the end position of a rope-driven continuum robot based on vision according to claim 1, characterized in that: The three detection points set on the disc base are three adjacent hexagonal screw positions among the six hexagonal screw positions equally distributed along the circumference of the disc base.

3. The method for detecting the end position of a rope-driven continuum robot based on vision according to claim 1, characterized in that: The intrinsic and extrinsic parameter matrices and distortion coefficients of the binocular camera are obtained by calibrating the binocular camera using the MATLAB toolbox.

4. The method for detecting the end position of a cable-driven continuum robot based on vision according to claim 1, characterized in that: The method of obtaining the bending angle and the rotation angle of the rope-driven continuum robot by a stereo geometry method based on the obtained three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system includes: Obtaining the three-dimensional position coordinates of a first intermediate point, a second intermediate point, a third intermediate point, a fourth intermediate point, a fifth intermediate point, and a sixth intermediate point in the binocular camera coordinate system according to the three-dimensional position coordinates of the first target detection point, the second target detection point, the third target detection point, and the fourth target detection point in the binocular camera coordinate system; wherein the first intermediate point, the second intermediate point, the third intermediate point, the fourth intermediate point, and the fifth intermediate point are located on a disk base, the disk base is arranged above and parallel to the disk base, the first intermediate point is the center point of the disk base, a line between the first intermediate point and the center point of the disk base, a line between the second intermediate point and the second target detection point, and a line between the third intermediate point and the fourth target detection point are all perpendicular to the plane of the disk base, the fourth intermediate point is on the reverse extension line of the line between the first intermediate point and the third intermediate point, the fifth intermediate point is the projection of the first target detection point on the plane of the disk base, and the sixth intermediate point is the midpoint between the first intermediate point and the first target detection point; Calculate the bending angle of the rope-driven continuum robot using the law of cosines based on the three-dimensional position coordinates of the first, fifth, and sixth intermediate points in the binocular camera coordinate system; the bending angle is the angle formed by the intersection of a plane direction including the center of the terminal sphere and perpendicular to the central axis of the robot and the line connecting the first and fifth intermediate points; Determine whether there is an intersection between the line segment formed by connecting the second intermediate point and the fifth intermediate point and the line segment formed by connecting the third intermediate point and the fourth intermediate point. If so, calculate the rotation angle of the rope-driven continuum robot based on the three-dimensional position coordinates of the first intermediate point, the third intermediate point and the fifth intermediate point in the binocular camera coordinate system and the cosine theorem; if not, calculate the mutual angle of the rotation angle of the rope-driven continuum robot based on the three-dimensional position coordinates of the first intermediate point, the third intermediate point and the fifth intermediate point in the binocular camera coordinate system and the cosine theorem; the rotation angle is the angle formed by the intersection of the line connecting the first intermediate point and the third intermediate point and the line connecting the first intermediate point and the fifth intermediate point; the mutual angle is equal to the difference between 2π and the rotation angle.

Images