Coaxial rope-driven manipulator configuration detection method and device based on binocular vision

Abstract

The present invention relates to a method and device for detecting the configuration of a coaxial rope-driven manipulator based on binocular vision. The coaxial rope-driven manipulator includes multiple joints. The method includes obtaining a first image and a second image of the coaxial rope-driven manipulator taken by a binocular camera; calculating the center coordinates of each of the joints according to the first image and the second image; and determining the configuration of the coaxial rope-driven manipulator according to the center coordinates of each of the joints. The present invention realizes accurate detection of the configuration of the coaxial rope-driven manipulator, provides strong support for the operation of the manipulator in a narrow space, simplifies the detection process, reduces the dependence on sensors and complex models, reduces costs and maintenance difficulties, and is suitable for different types of coaxial rope-driven manipulators and different application scenarios.

Description

Technical Field

[0001] The present invention relates to the technical field of computer vision, and in particular to a method and device for detecting the configuration of a coaxial rope-driven manipulator based on binocular vision. Background Art

[0002] With the development of medical technologies such as minimally invasive surgery and oral inspection, higher requirements are placed on robotic systems operating in confined spaces. Coaxial rope-driven manipulators have shown great application potential in these fields due to their unique flexibility and redundancy. However, due to the structural characteristics of coaxial rope-driven manipulators, configuration detection in confined spaces has become a technical challenge.

[0003] At present, the configuration detection of coaxial rope-driven manipulators mainly relies on traditional measurement methods, such as sensor arrays, mechanical models of manipulators, etc. However, these methods have many limitations in practical applications, such as the complexity of sensor layout and the inaccuracy of mechanical models, which makes it difficult to meet the requirements of detection accuracy and robustness. Summary of the invention

[0004] The main purpose of the present invention is to provide a method and device for detecting the configuration of a coaxial rope-driven manipulator based on binocular vision, so as to solve the problem of low configuration detection accuracy of the coaxial rope-driven manipulator in the prior art.

[0005] To achieve the above object, the present invention provides a first aspect of a binocular vision-based coaxial rope-driven manipulator configuration detection method, wherein the coaxial rope-driven manipulator comprises a plurality of joints, and the method comprises:

[0006] Acquire a first image and a second image of the coaxial rope-driven robotic arm taken by a binocular camera;

[0007] Calculating the center coordinates of each of the joints according to the first image and the second image;

[0008] The configuration of the coaxial rope-driven robotic arm is determined according to the central coordinates of each of the joints.

[0009] Further, the joint comprises a first connection portion and a second connection portion connected to each other, the second connection portion comprises an opposite top surface and a bottom surface, the top surface is a surface adjacent to the first connection portion of the same joint, and the bottom surface is a surface adjacent to another joint;

[0010] Two adjacent joints are connected by inserting a first connection part of one joint into a second connection part of another joint; a plurality of the joints are connected in sequence to form the coaxial rope-driven robotic arm;

[0011] The second connecting portion is cylindrical, and a preset identification pattern is provided on the outer side surface of the second connecting portion;

[0012] The binocular camera is arranged at a preset distance from the side of the coaxial rope-driven robotic arm, and the side of the coaxial rope-driven robotic arm is the side where the binocular camera can capture the preset identification pattern.

[0013] Furthermore, the preset identification pattern is a circle, and the step of respectively calculating the center coordinates of each of the joints according to the first image and the second image comprises:

[0014] According to the first image and the second image, respectively determining whether the axis line of each of the preset identification patterns and the midline of the second connection part of the joint corresponding to each of the preset identification patterns coincide with each other, and determining whether the midline of the second connection part of the joint corresponding to each of the preset identification patterns coincides with the axis line of the binocular camera;

[0015] According to the judgment result, the center coordinates of the joints corresponding to the preset identification patterns are calculated.

[0016] Furthermore, the step of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0017] If the axis of the preset identification pattern coincides with the midline of the second connection portion of the joint corresponding to the preset identification pattern, the spatial coordinate a (x a ,y a ,z a ), the lowest spatial coordinate c(x c ,y c ,z c ), the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d );

[0018] By the top spatial coordinate a(x a ,y a ,z a ) and the lowest spatial coordinate c(x c ,y c ,z c ), calculate the center coordinates of the first marker and moving the first identification center coordinates by a distance r in the direction of the normal vector of the preset identification pattern to obtain the center coordinates of the joint corresponding to the preset identification pattern, wherein the direction of the normal vector is perpendicular from the center of the preset identification pattern to the central axis of the second connection part, and r is the outer diameter of the cross section of the second connection part; or,

[0019] By the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), calculate the center coordinates of the second marker and moving the center coordinates of the second identification along the normal vector direction of the preset identification pattern by a distance r to obtain the center coordinates of the joint corresponding to the preset identification pattern; or,

[0020] The first mark center coordinates and the second mark center coordinates are averaged to obtain the third mark center coordinates. The third identification center coordinate is moved a distance r along the normal vector direction of the preset identification pattern to obtain the center coordinate of the joint corresponding to the preset identification pattern.

[0021] Furthermore, the step of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0022] If the axis line of the preset identification pattern and the midline of the second connection part of the joint corresponding to the preset identification pattern do not coincide, but the midline of the second connection part of the joint corresponding to the preset identification pattern coincides with the axis of the binocular camera, then the coordinate A (x A ,y A ,z A ), the coordinates of the lower endpoint B(x B ,y B ,z B ), and the coordinates M(x M ,y M ,z M );

[0023] Calculate the coordinates of the intersection point O(x O ,y O ,z O ), wherein the line segment AM is formed by connecting the upper endpoint and the center point of the arc, and the line segment BM is formed by connecting the lower endpoint and the center point of the arc;

[0024] Move the intersection coordinate o (x, y, z) along the central axis of the second connection part, and from the bottom surface of the second connection part of the same joint to the top surface of the second connection part, a distance h / 2 to obtain the center coordinates of the joint, where h is the generatrix length of the second connection part.

[0025] Furthermore, the step of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0026] If the axis line of the preset identification pattern and the midline of the second connection part of the joint corresponding to the preset identification pattern do not coincide with each other, and the midline of the second connection part of the joint corresponding to the preset identification pattern and the axis line of the binocular camera do not coincide with each other, then the coordinate A (x A ,y A ,z A ), the coordinate B (x B ,y B ,z B ), and the coordinates M(x M ,y M ,z M );

[0027] Calculate the coordinates of the intersection point O(x O ,y O ,z O ), wherein the line segment AM is formed by connecting the upper endpoint and the center point of the arc, and the line segment BM is formed by connecting the lower endpoint and the center point of the arc;

[0028] Get the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), calculate the vector The vector Parallel to the central axis of the two connecting parts, and the first connecting part is located to the left of the second connecting part;

[0029] According to the intersection coordinates O(x O ,y O ,z O ) and the vector Obtaining the direction vector of the central axis of the second connecting portion and converting it into a central axis straight line expression;

[0030] And according to the spatial coordinate a(x a ,y a ,z a ), the lowest spatial coordinate c(x c ,y c ,z c ), the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), calculate the center of the circle passing through the preset identification pattern and perpendicular to and The normal vector of the determined plane, where and converting the normal vector into a normal line expression;

[0031] According to the central axis straight line expression and the normal line straight line expression, the intersection coordinates of the central axis and the normal line are calculated, and the intersection coordinates are the center coordinates of the joint corresponding to the preset identification pattern.

[0032] Furthermore, the step of respectively calculating the center coordinates of each of the joints according to the first image and the second image comprises:

[0033] Performing image preprocessing on the first image and the second image to obtain grayscale images;

[0034] Extracting the edge of each of the joints from the grayscale image;

[0035] Draw a circumscribed rectangle circumscribing the edges of the joint;

[0036] The centroid coordinates of the circumscribed rectangle are calculated, and the centroid coordinates are used as the center coordinates of the joint.

[0037] Furthermore, the step of determining the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints further includes:

[0038] The spatial position of each joint is determined according to the central coordinates of each joint and the central axis of the second connecting part of each joint.

[0039] Furthermore, after the step of determining the spatial posture of each joint according to the central coordinates of each joint and the central axis of the second connecting portion of each joint, the method further includes:

[0040] According to the spatial posture of each joint, the configuration of the coaxial rope-driven robotic arm is presented in a three-dimensional diagram.

[0041] A second aspect of the present invention provides a coaxial rope-driven manipulator configuration detection device based on binocular vision, wherein the coaxial rope-driven manipulator comprises a plurality of joints, and the device comprises:

[0042] An acquisition module, for acquiring a first image and a second image of the coaxial rope-driven manipulator taken by a binocular camera;

[0043] A center coordinate calculation module, used to calculate the center coordinates of each of the joints according to the first image and the second image;

[0044] The configuration determination module is used to determine the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints.

[0045] The present invention provides a binocular vision-based coaxial rope-driven robotic arm configuration detection method and device. The method obtains a first image and a second image of the coaxial rope-driven robotic arm taken by a binocular camera, calculates the center coordinates of each joint according to the first image and the second image, and determines the configuration of the coaxial rope-driven robotic arm according to the center coordinates of each joint, thereby achieving accurate detection of the coaxial rope-driven robotic arm configuration, providing strong support for the operation of the robotic arm in a small space, and simplifying the detection process, reducing dependence on sensors and complex models, reducing costs and maintenance difficulties, and being suitable for different types of coaxial rope-driven robotic arms and different application scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a schematic flow chart of a method for detecting a configuration of a coaxial rope-driven manipulator based on binocular vision in one embodiment of the present invention;

[0047] Figure 2 is a schematic side structural diagram of a coaxial rope-driven manipulator in one embodiment of the present invention;

[0048] Figure 3 is a schematic side structural diagram of a joint of a coaxial rope-driven manipulator in one embodiment of the present invention;

[0049] Figure 4 is a structural schematic diagram of the positional relationship among a preset identification pattern, a joint, and a binocular camera in one embodiment of the present invention;

[0050] Figure 5 is a schematic diagram of a flow chart of calculation of the center coordinates of a joint in one embodiment of the present invention;

[0051] Figure 6 is a schematic diagram of the spatial relationship of the central axis of the second connecting portion space cylinder in one embodiment of the present invention;

[0052] Figure 7is a schematic diagram of the geometric relationship between a point on a cylinder and the central axis in one embodiment of the present invention;

[0053] Figure 8 It is a schematic block diagram of the structure of a coaxial rope-driven manipulator configuration detection device based on binocular vision in one embodiment of the present invention;

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

[0055] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0056] Reference Figure 1 and Figure 2 The embodiment of the present invention discloses a configuration detection method of a coaxial rope-driven manipulator based on binocular vision, wherein the coaxial rope-driven manipulator comprises a plurality of joints 1, and the method comprises:

[0057] S1, acquiring a first image and a second image of a coaxial rope-driven robotic arm taken by a binocular camera;

[0058] S2. Calculate the center coordinates of each of the joints according to the first image and the second image;

[0059] S3. Determine the configuration of the coaxial rope-driven robotic arm according to the central coordinates of each of the joints.

[0060] In this embodiment, in the above step S1, binocular vision technology can obtain the three-dimensional coordinates of the object in real time and accurately by simulating the working principle of the human eye, and provide the robot with rich visual perception information.

[0061] The installation position of the binocular camera should ensure that the binocular camera can clearly capture all or key parts of the joints of the coaxial rope-driven manipulator, and the field of view of the two cameras should have enough overlapping area for stereo matching. Usually, the camera is installed on the side or above the manipulator, depending on the working environment and structural characteristics of the manipulator. After the installation is completed, the binocular camera is calibrated. The purpose of calibration is to determine the relative position relationship between the two cameras (including rotation and translation) and their internal parameters (such as focal length, optical center, etc.) to ensure the accuracy of subsequent image processing. This is usually done by shooting a calibration plate with a known pattern and applying a specific algorithm. Start the binocular camera and shoot the current state of the coaxial rope-driven manipulator. Since the binocular camera has two lenses, they will simultaneously capture two slightly different images of the manipulator, namely the first image and the second image.

[0062] In the above step S2, the first image and the second image are preprocessed, including denoising, contrast enhancement, grayscale and other operations to improve image quality and simplify subsequent processing steps. The features of the joints are extracted from the image by image processing techniques (such as edge detection, corner detection, template matching, etc.). The joint features in the first image and the second image are matched using the stereoscopic vision principle of the binocular camera. By comparing the parallax of corresponding points in the two images, the positions of these joint feature points in three-dimensional space can be calculated. Then, the center coordinates of each joint are calculated by the positions of these joint feature points.

[0063] In the above step S3, the method for determining the configuration of the coaxial rope-driven robotic arm includes reconstructing the configuration of the coaxial rope-driven robotic arm in three-dimensional space using the three-dimensional center coordinates of each joint, and the configuration reconstruction step usually involves connecting the joint points into line segments or curves to represent the skeleton structure of the robotic arm; analyzing the reconstructed configuration, calculating the relative position, angle or distance between the joints, etc., to determine the current state of the robotic arm (such as bending degree, extension length, etc.); outputting the configuration information obtained by analysis in a predefined form, such as displaying it on a screen, storing it in a file, or sending it to a control system for further processing.

[0064] This embodiment realizes accurate detection of the configuration of the coaxial rope-driven robotic arm through the above steps, provides strong support for the operation of the robotic arm in a small space, simplifies the detection process, reduces dependence on sensors and complex models, reduces costs and maintenance difficulties, and is suitable for different types of coaxial rope-driven robotic arms and different application scenarios.

[0065] Specifically, refer to Figure 1 and Figure 2 The joint 1 of the coaxial rope-driven manipulator comprises a first connection part 11 and a second connection part 12 connected to each other, the second connection part comprises a top surface 121 and a bottom surface 122 opposite to each other, the top surface 121 is a surface adjacent to the first connection part 11 of the same joint 1, and the bottom surface 122 is a surface adjacent to another joint 1; two adjacent joints 1 are connected by inserting the first connection part 11 of one joint 1 into the second connection part 12 of the other joint 1; a plurality of the joints 1 are connected in sequence to form the coaxial rope-driven manipulator; the second connection part 12 is cylindrical, and a preset identification pattern 13 is provided on the outer side of the second connection part 12; in order to improve the recognition accuracy and efficiency of the binocular camera to the joint, a preset identification pattern 13 is provided on the outer side of the second connection part 12. The preset identification pattern 13 can be a simple geometric shape (such as a circle, a square, etc.), or a pattern with specific coding information.

[0066] The binocular camera is arranged at a preset distance on the side of the coaxial rope-driven manipulator, and the side of the coaxial rope-driven manipulator is a side where the binocular camera can capture the preset identification pattern. The preset distance should be selected to ensure that the binocular camera can clearly capture all joints and the corresponding preset identification patterns, and the fields of view of the two cameras have enough overlapping areas for stereo matching.

[0067] In a specific embodiment, the preset identification pattern is a circle, and the step S2 of respectively calculating the center coordinates of each of the joints according to the first image and the second image comprises:

[0068] S21, according to the first image and the second image, respectively determining whether the axis line of each of the preset identification patterns and the midline of the second connection part of the joint corresponding to each of the preset identification patterns coincide with each other, and determining whether the midline of the second connection part of the joint corresponding to each of the preset identification patterns coincides with the axis line of the binocular camera;

[0069] S22. Calculate the center coordinates of the joints corresponding to the preset identification patterns according to the judgment results.

[0070] In this embodiment, a circle is preferably used as the preset identification pattern because the circle has the advantages of being easy to identify and calculate. A circular identification pattern is provided on the outer side of the second connection part of each joint to assist the binocular camera in image processing and three-dimensional coordinate calculation. In this embodiment, step S21 calculates the axis line of the preset identification pattern and the center line of the second connection part by performing image processing on the first image and the second image. The axis line of the preset identification pattern is a straight line passing through the center of the circle of the preset identification pattern and perpendicular to the normal plane at the center of the circle of the preset identification pattern. The second connection part of the joint is approximately rectangular in the first or second image taken by the binocular vision camera, wherein the line is a straight line passing through the center of the rectangle and perpendicular to the center axis of the second connection part, the second connection part is cylindrical, and the center axis of the second connection part is a straight line passing through the center of the cross section (circle) of the second connection part and perpendicular to the cross section. The axis of the above-mentioned binocular camera is the main optical axis of the lens, which can be determined by the parameters of the binocular camera (such as manually set height, lens size parameters, etc.). Through the positional relationship of these three, different methods are selected to solve the joint center coordinates.

[0071] In a specific embodiment, the step S22 of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0072] S2211, reference Figure 4(a) and 4(b), if the axis line of the preset identification pattern coincides with the midline of the second connection part of the joint corresponding to the preset identification pattern, then the spatial coordinate a (x a ,y a ,z a ), the lowest spatial coordinate c(x c ,y c ,z c ), the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ); the spatial coordinates of specific points on the identification graphic (such as the top, bottom, leftmost, and rightmost) can be calculated by using the stereo vision technology of the binocular camera, using the first image and the second image taken by two cameras from different angles, through feature matching and triangulation methods.

[0073] S2212, through the top spatial coordinate a(x a ,y a ,z a ) and the lowest spatial coordinate c(x c ,y c ,z c ), calculate the center coordinates of the first marker And move the first identification center coordinate along the normal vector direction of the preset identification pattern by a distance r to obtain the center coordinate of the joint corresponding to the preset identification pattern, wherein the direction of the normal vector is perpendicular to the center of the circle of the preset identification pattern and points to the central axis of the second connection part, and r is the cross-sectional outer diameter of the second connection part; wherein the cross-sectional area of ​​the second connection part is a circular cross-sectional area. Or,

[0074] S2213, through the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), calculate the center coordinates of the second marker and moving the second identification center coordinates by a distance r along the normal vector direction of the preset identification pattern to obtain the center coordinates of the joint corresponding to the preset identification pattern, wherein the direction of the normal vector and the definition of r are the same as those in S2203; or,

[0075] S2214: Calculate the average of the first marker center coordinates and the second marker center coordinates to obtain the third marker center coordinates. The third identification center coordinate is moved by a distance r along the normal vector direction of the preset identification pattern to obtain the center coordinate of the joint corresponding to the preset identification pattern, wherein the direction of the normal vector and the definition of r are the same as S2203. Step S2204 can further improve the calculation accuracy of the center coordinate.

[0076] In a specific embodiment, the step S22 of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0077] If the axis line of the preset identification pattern coincides with the midline of the second connection portion of the joint corresponding to the preset identification pattern, but the midline of the second connection portion of the joint corresponding to the preset identification pattern does not coincide with the axis line of the binocular camera, refer to Figure 4 (b) then,

[0078] According to the spatial coordinate a(x a ,y a ,z a ), the lowest spatial coordinate c(x c ,y c ,z c ), the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), the direction vector from the top of the preset logo graphic to the bottom of the preset logo graphic is calculated as And the direction vector from the leftmost side of the preset logo pattern to the rightmost side of the preset logo pattern is calculated as

[0079] Calculate the normal vector of the preset logo graphic as The direction of the normal vector is perpendicular to the center of the preset identification pattern and points to the central axis of the cylinder; and They represent the modulus of the corresponding vectors, θ is the angle between the two vectors, Represents a perpendicular and the unit normal vector of the determined plane;

[0080] The center space coordinates and normal vector of the known landmark point Use the straight-line point method to represent the axis line of the marker point.

[0081] Since the axis of the preset identification pattern coincides with the center line of the second connection part, it is only necessary to move the spatial coordinates of the center of the marker point along the axis of the preset identification pattern by the radius of the cross section (circular surface) of the second connection part. The coordinates of a point on the axis of the preset identification pattern are P(x1, y1, z1), and the new coordinates after moving a distance r on the straight line are Q(x2, y2, z2), which are the center coordinates of the joint. The equation of the straight line after the direction vector of the straight line is simplified is:

[0082] ax+by+cz+d=0

[0083] Where a, b, c are the direction vectors of the line, and d is the intercept.

[0084] Assuming that the coordinates of the general point on the axis of the preset logo figure are (x, y, z), the relationship between point P and the general point can be expressed as:

[0085] (x,y,z)=(x1+ar,y1+br,z1+cr).

[0086] Substituting the coordinates of this point into the general expression of the axis line, the relationship is:

[0087] a 2 r+b 2 r+c 2 r=-(ax1+by1+cz1+d)

[0088] According to the above equation, the coordinates of the new coordinate point Q (x2, y2, z2) after the move can be obtained as:

[0089]

[0090] In a specific embodiment, the step S22 of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0091] S2231, reference Figure 4 (c) If the axis line of the preset identification pattern and the midline of the second connection part of the joint corresponding to the preset identification pattern do not coincide with each other, but the midline of the second connection part of the joint corresponding to the preset identification pattern coincides with the axis line of the binocular camera, then the coordinate A (x A ,y A ,z A ), the coordinates of the lower endpoint B(x B ,y B ,z B ), and the coordinates M(x M,y M ,z M );

[0092] S2232, calculate the intersection coordinates O (x) of the first perpendicular bisector of line segment AM and the second perpendicular bisector of line segment BM. O ,y O ,z O ), wherein the line segment AM is formed by connecting the upper endpoint and the center point of the arc, and the line segment BM is formed by connecting the lower endpoint and the center point of the arc;

[0093] S2233, the intersection coordinate O(x O ,y O ,z O ) along the central axis of the second connection part, and from the bottom surface of the second connection part of the same joint to the top surface of the second connection part, move a distance h / 2 to obtain the center coordinates of the joint, where h is the generatrix length of the second connection part.

[0094] Specifically, in this embodiment, the first perpendicular bisector S1 passes through the midpoint of the line segment AM. The direction vector of line segment AM is From the basic properties of the perpendicular bisector, it can be concluded that the direction vector of line segment AM is the normal vector of the perpendicular bisector S1, that is, The equation of the perpendicular bisector S1 can be obtained by the straight-line-point method as follows:

[0095]

[0096] Similarly, the perpendicular bisector S2 passes through the midpoint of the line segment BM. The direction vector of line segment BM is The equation of the perpendicular bisector S2 can be obtained by the straight-line-point method as follows:

[0097]

[0098] To solve the center O(x O ,y O ,z O ), we need to apply the basic properties of the circle. From the fact that the distances from each point on the circle to the center are equal, we can get:

[0099] d A-O =d B-O =d C-O

[0100] Substitute the coordinates of each point into the numerical calculation and get:

[0101]

[0102] In order to facilitate the conversion of coordinates and the calculation of numerical values, the perpendicular bisectors S1 and S2 are simplified into general expressions of plane straight lines. Assumptions:

[0103]

[0104] In this way, complex problems are transformed into simple equation solving problems, and the perpendicular bisectors S1 and S2 are further simplified to:

[0105]

[0106] Since they are three coordinate points in space, as long as they are not in a straight line, the plane position of the circle can be determined. Therefore, the third equation should be a plane constraint equation, that is:

[0107]

[0108] Similarly, simplify the plane constraint equation into a general homogeneous form, assuming:

[0109]

[0110] The general formula of the plane constraint equation can be obtained:

[0111] A3x+B3y+C3z+D3=0

[0112] Combining the formulas, we can form a set of linear algebraic equations about the spatial coordinates of the center of the circle:

[0113]

[0114] According to Cramer's law, the coordinates of the center of the bottom surface of the space cylinder O(x O ,y O ,z O ):

[0115]

[0116] Since the midline of the joint is consistent with the axis of the camera, the direction vector of the midline of the second joint of the joint can be determined The axis of the camera is perpendicular to each other, and the direction vector of the axis of the second connecting part can be expressed as vector It is concluded that:

[0117]

[0118] The center coordinates of the joint can be moved by a distance h / 2 from the center coordinates of the bottom circle along the direction vector of the center axis of the second connection part.

[0119] In a specific embodiment, the step S22 of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result includes:

[0120] S2241, reference Figure 4 (d) If the axis line of the preset identification pattern and the midline of the second connection part of the joint corresponding to the preset identification pattern do not coincide with each other, and the midline of the second connection part of the joint corresponding to the preset identification pattern and the axis line of the binocular camera do not coincide with each other, then the coordinate A (x A ,y A ,z A ), the coordinate B (x B ,y B ,z B ), and the coordinates M(x M ,y M ,z M );

[0121] S2242, calculate the intersection coordinates O (x) of the first perpendicular bisector of line segment AM and the second perpendicular bisector of line segment BM. O ,y O ,z O ), wherein the line segment AM is formed by connecting the upper endpoint and the center point of the arc, and the line segment BM is formed by connecting the lower endpoint and the center point of the arc;

[0122] S2243, obtaining the leftmost spatial coordinate b (x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), calculate the vector The vector Parallel to the central axis of the two connecting parts, and the first connecting part is located to the left of the second connecting part;

[0123] S2244, according to the intersection coordinates O(x O ,y O ,z O ) and the vector Obtaining the direction vector of the central axis of the second connecting portion and converting it into a central axis straight line expression;

[0124] S2245, and according to the spatial coordinate a(x a ,y a ,za ), the lowest spatial coordinate c(x c ,y c ,z c ), the leftmost spatial coordinate b(x b ,y b ,z b ) and the rightmost spatial coordinate d(x d ,y d ,z d ), calculate the center of the circle passing through the preset identification pattern and perpendicular to and The normal vector of the determined plane, where and converting the normal vector into a normal line expression;

[0125] S2246. According to the linear expression of the central axis and the linear expression of the normal line, the intersection coordinates of the central axis and the normal line are calculated, and the intersection coordinates are the center coordinates of the joint corresponding to the preset identification pattern.

[0126] Specifically, in this embodiment, the position information of the bottom surface of the cylinder is first obtained by a binocular camera, and the coordinates A (x A ,y A ,z A ), the coordinates of the lower endpoint B(x B ,y B ,z B ), and the coordinates M(x M ,y M ,z M Since the three points are not on the same straight line, the three points in space are used to determine the center of the circle O(x O ,y O ,z O ). Secondly, the position information of the landmark point is obtained through the binocular camera, and the vector Thus, the direction vector of the second connection part of the joint is obtained Finally, in order to distinguish the relationship between the center of the circle and the straight line point, the coordinates of the center of the circle are specialized into O(x0, y0, z0), and the point-wise expression is used to describe the spatial straight line to obtain the expression of the central axis of the second connection part of the joint:

[0127]

[0128] In order to facilitate calculation and solution, a parameter ε is introduced to transform the straight line into:

[0129]

[0130] Then, the positions of the marker points are detected by binocular vision to obtain the spatial coordinates of the four points a(xa ,y a ,z a )、b(x b ,y b ,z b )、c(x c ,y c ,z c ) and d(x d ,y d ,z d ). Then, the center coordinates of the marker point are obtained. In order to distinguish the relationship between the center point and the straight line point, the center point coordinates are specialized into O1(x 01 ,y 01 ,z 01 ). Secondly, the direction vector is calculated through these spatial coordinate information and direction vector Cross product to get the normal vector Finally, the point-to-point formula is used to describe the space straight line to obtain the expression of the straight line where the normal line of the marker point is located:

[0131]

[0132] Similarly, a parameter η is introduced to transform the straight line into:

[0133]

[0134] The parametric equations of the two straight lines are combined, and the intersection of the two straight lines obtained by solving the problem is the required point, which is the spatial coordinate of the center point of the second connection part of the joint.

[0135] In a specific embodiment, referring to Figure 5 The step S2 of calculating the center coordinates of each joint according to the first image and the second image comprises:

[0136] S23, performing image preprocessing on the first image and the second image to obtain grayscale images;

[0137] S24, extracting the edge of each joint from the grayscale image;

[0138] S25, drawing a circumscribed rectangle circumscribed to the edge of the joint;

[0139] S26. Calculate the centroid coordinates of the circumscribed rectangle, and use the centroid coordinates as the center coordinates of the joint.

[0140] Specifically, through the above step S23, the noise and unnecessary details in the image can be reduced, and the features of the joint area can be enhanced to facilitate subsequent edge detection and shape analysis. The above preprocessing process includes image graying, image filtering, Gaussian blur, image denoising, corner detection, edge detection, etc.

[0141] In the above step S24, the edge of the joint is extracted from the preprocessed grayscale image. The edge is the place where the brightness changes most dramatically in the image, usually corresponding to the outline of the object. Commonly used edge detection algorithms include Canny edge detector, Sobel operator, Prewitt operator, etc. These algorithms can identify the area where the brightness changes significantly in the image, that is, the edge of the joint. In order to remove some unnecessary edges (such as edges caused by noise), the edge detection results can also be thresholded to retain only those edges whose intensity exceeds a certain threshold.

[0142] In step S25, the above-mentioned bounding rectangle is preferably a minimum bounding rectangle. Exemplarily, the method for drawing a minimum bounding rectangle includes the following steps. In the first step, an algorithm for a simple bounding rectangle of a polygon is used. A simple bounding rectangle refers to a bounding rectangle whose sides are parallel to the x-axis or the y-axis, and the maximum value x of the edge coordinate point in the x-axis and y-axis directions is extracted. max ,y max and the minimum value x min ,y min The simple bounding rectangle is probably not the minimum bounding rectangle, but it is very easy to find, which paves the way for the subsequent steps. Based on the symmetry and geometric principles of the rotating body, the coordinate values ​​corresponding to each pixel on the rotated image are solved. The length of the outline bounding rectangle L = x max -x min , width W = y max -y min , area S = L × W, rotation angle θ = 0°, current minimum area S min =S. Step 2: Rotate the target joint contour clockwise by a small angle Δθ, then the current angle θ = Δθ. Find the simple circumscribed rectangle after each degree of rotation, and record the coordinates of the simple circumscribed rectangle points and the degree of rotation at this time. If the current area is smaller than the initial S min , then let the current area be the new minimum area S min Otherwise, keep S min Step 3: Repeat step 2 until θ>90°, the minimum area at this time is the minimum circumscribed rectangle area S min , get the vertex coordinates and rotation angle of the simple circumscribed rectangle.

[0143] In the above step S26, after obtaining the pixel coordinates of the four corner points of the minimum circumscribed rectangle of the target joint, the coordinates of its centroid can be obtained.

[0144] In a specific embodiment, the step S3 of determining the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints further includes:

[0145] S31. Determine the spatial position of each joint according to the central coordinates of each joint and the central axis of the second connection part of each joint.

[0146] The center coordinates of each joint are calculated by the method in the above embodiment, and the central axis information of the second connection part of each joint is determined. Since the shape of the second connection part of the coaxial rope-driven manipulator joint is similar to a space cylinder, in order to simplify the spatial shape of the joint so as to solve the position and posture of the joint in space, a cylinder is used for geometric calculation, and the central axis of the joint is simplified to the central axis of the space cylinder of the second connection part.

[0147] The method for determining the spatial pose of the joint, for example, can adopt the following calculation method to establish a local coordinate system for each joint, for example, with the center of the joint as the origin, and the direction of the central axis as the positive direction of a certain coordinate axis (such as the Z axis). According to the design or actual measurement, the directions of other coordinate axes (such as the X-axis and the Y-axis) are determined. Using the central coordinates and local coordinate system of the joint, the transformation matrix from the global coordinate system to the local coordinate system of each joint can be calculated. This transformation matrix contains two parts: translation (given by the central coordinates) and rotation (determined by the direction of the central axis and the directions of other coordinate axes). Through the transformation matrix, the points or vectors in the local coordinate system of each joint can be converted to the global coordinate system, thereby determining the exact position and direction of each joint in three-dimensional space, that is, the spatial pose.

[0148] For another example, the central axis expression of the second connecting part space cylinder can also be obtained by the following method:

[0149]

[0150] Where q0(x0,y0,z0) is a known point on the central axis of the cylinder. The geometric relationship of the central axis in space is as follows Figure 6 shown.

[0151] The unit vector in the direction of the central axis is:

[0152]

[0153] Wherein, α is the angle between the projection l' of the central axis l on the plane XOY and the X-axis.

[0154] β is the angle between the central axis l and the plane XOY.

[0155] Assume that there is an unknown point p(x,y,z) on the cylinder, and make pm⊥l, such as Figure 7shown.

[0156] From this, we can see that the distance from the unknown point p(x,y,z) to the centerline of the cylinder is:

[0157] d p-l = r = pqsin(θ)

[0158] Among them, θ is the angle between pq0 and the central axis l.

[0159] According to the Euclidean distance formula and the principle of linear symmetry equation, we can get:

[0160]

[0161] The direction vector of line segment pq0 is:

[0162]

[0163] Use the law of cosines to solve for the angle θ in the constructed triangle:

[0164]

[0165] In the constructed triangle, we get from the geometric relationship of the figure:

[0166]

[0167] The distance of PM can be obtained from the Euclidean distance formula:

[0168]

[0169] In the constructed triangle, according to the geometric relationship, we can get:

[0170]

[0171] Through the above two different methods, we get two different expressions of cosθ, which are combined to get:

[0172]

[0173] Simplify and solve the cylinder equation expression:

[0174]

[0175] From the unit vector of the central axis, we know that l 2 +m 2 +n 2 =1, so simplifying it gives:

[0176]

[0177] In a specific embodiment, after the step S31 of determining the spatial position of each joint according to the central coordinates of each joint and the central axis of the second connection portion of each joint, the step further includes:

[0178] S32. Presenting the configuration of the coaxial rope-driven robotic arm in a three-dimensional diagram according to the spatial posture of each of the joints.

[0179] In this embodiment, a three-dimensional model of the coaxial rope-driven robotic arm can be created using three-dimensional modeling software or a graphics library (such as OpenGL, DirectX, Unity3D, etc.).

[0180] The spatial pose of each joint calculated in step S31 is applied to the corresponding three-dimensional coordinates in the three-dimensional model, so as to present the configuration of the coaxial rope-driven manipulator in the three-dimensional graph. The adjusted manipulator model can also be further rendered and displayed in a three-dimensional graphics environment.

[0181] Specifically, the method for solving the three-dimensional coordinates is as follows, and the matrix changes required to convert the pixel coordinates of a point into the world system coordinates are as follows.

[0182]

[0183] Converting the formula into general form:

[0184]

[0185] For a special spatial point P, the image plane points p1 and p2 obtained by the left and right cameras are the same corresponding points of the special point P. The corresponding rule satisfies the above formula, that is:

[0186]

[0187] Where (U1, V1, 1) and (U2, V2, 1) are the image coordinate system homogeneous coordinates of points p1 and p2 in their respective camera images, (X W ,Y W ,Z W ,1) is the homogeneous coordinate of point P in the world coordinate system.

[0188] Eliminate Z from the two equations c1 and Z c2 Then we get two sets of equations about unknown variables:

[0189]

[0190] The problem of solving this unknown number is an overdetermined problem. At this time, the solution of Ax=b does not exist, so we need to solve the least squares problem. However, it is difficult to solve the inverse matrix of this matrix, so we use the singular value decomposition (SVD) method to solve the least squares problem of the spatial point.

[0191] According to this method, the matrix needs to be decomposed, that is, the target matrix is ​​decomposed into:

[0192] A=UΣV T

[0193] Among them, U is a 3×3 matrix, Σ is a 3×4 singular value matrix, and V is a 4×4 matrix.

[0194] For the decomposed matrix, every element on the main diagonal of the singular value matrix Σ is a singular value and all elements except the diagonal elements are 0. Both the matrix U and the matrix V are unitary matrices.

[0195] For the target matrix A, you need to construct a square matrix A T A is then decomposed into its own features. The eigenvalues ​​λ i With each eigenvector v i for:

[0196] (A T A)v i =λ i v i

[0197] All feature vectors v i Form a square matrix, which is the matrix V of the target matrix decomposition, containing each eigenvector v i are the right singular vectors of the target matrix A.

[0198] Similarly, for the target matrix A, it is also necessary to construct a square matrix AA T So as to perform the eigendecomposition. The eigenvalues ​​λ i With each eigenvector u i for:

[0199] (AA T ) i =λ i u i

[0200] All eigenvectors u i Form a square matrix, which is the matrix U of the target matrix decomposition, containing each eigenvector u i are the left singular vectors of the target matrix A.

[0201] Combining the above three formulas, we can get:

[0202]

[0203] Each singular value σ i All form the diagonal elements of the singular value matrix Σ.

[0204] The singular value matrix Σ is a diagonal matrix, and its generalized inverse matrix Σ + The elements of are the inverses of the matrix Σ. Based on the results of the singular value decomposition, the generalized inverse matrix A of the target matrix A can be easily calculated: + ,Right now:

[0205] A + =(UΣV T ) + =(ΣV T ) + U -1 =VΣ + U T

[0206] The generalized inverse matrix A of the target matrix A is obtained by singular value decomposition (SVD) + , and for the spatial point P, the two straight lines starting from the left and right cameras through p1 and p2 must have an intersection in space, and it is the only solution. Therefore, the three-dimensional coordinates of the spatial point are obtained by solving the least squares problem.

[0207] Reference Figure 8 An embodiment of the present invention further provides a coaxial rope-driven manipulator configuration detection device based on binocular vision, wherein the coaxial rope-driven manipulator comprises a plurality of joints, and the device comprises:

[0208] An acquisition module 10 acquires a first image and a second image of the coaxial rope-driven robotic arm taken by a binocular camera;

[0209] A center coordinate calculation module 20, used to calculate the center coordinates of each of the joints according to the first image and the second image;

[0210] The configuration determination module 30 is used to determine the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints.

[0211] In this embodiment, for the specific implementation of each module in the above-mentioned device embodiment, please refer to the above-mentioned method embodiment, which will not be described in detail here.

[0212] An embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the above method is implemented. It can be understood that the computer-readable storage medium in this embodiment can be a volatile readable storage medium or a non-volatile readable storage medium.

[0213] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media provided by the present invention and used in the embodiments can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double-speed data rate SDRAM (SSRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM.

[0214] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, device, article or method including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, device, article or method. In the absence of further restrictions, an element defined by the sentence "includes a ..." does not exclude the presence of other identical elements in the process, device, article or method including the element.

[0215] The above description is only a preferred embodiment of the present invention, and does not limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made by using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for detecting the configuration of a coaxial rope-driven manipulator based on binocular vision, characterized in that: The coaxial rope-driven manipulator comprises a plurality of joints, and the method comprises: Acquire a first image and a second image of the coaxial rope-driven robotic arm taken by a binocular camera; Calculating the center coordinates of each of the joints according to the first image and the second image; Determining the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints; Wherein, the joint comprises a first connection portion and a second connection portion connected to each other, the second connection portion comprises a top surface and a bottom surface opposite to each other, the top surface is a surface adjacent to the first connection portion of the same joint, and the bottom surface is a surface adjacent to another joint; Two adjacent joints are connected by inserting a first connection part of one joint into a second connection part of another joint; a plurality of the joints are connected in sequence to form the coaxial rope-driven robotic arm; The second connecting portion is cylindrical, and a preset identification pattern is provided on the outer side surface of the second connecting portion; The binocular camera is arranged at a preset distance from the side of the coaxial rope-driven robotic arm, and the side of the coaxial rope-driven robotic arm is a side where the binocular camera can capture the preset identification pattern; Wherein, the preset identification pattern is a circle, and the step of respectively calculating the center coordinates of each of the joints according to the first image and the second image comprises: According to the first image and the second image, respectively determining whether the axis line of each of the preset identification patterns and the midline of the second connection part of the joint corresponding to each of the preset identification patterns coincide with each other, and determining whether the midline of the second connection part of the joint corresponding to each of the preset identification patterns coincides with the axis line of the binocular camera; According to the judgment result, the center coordinates of the joints corresponding to the preset identification patterns are calculated.

2. The binocular vision-based coaxial rope-driven manipulator configuration detection method according to claim 1 is characterized in that: The step of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result comprises: If the axis line of the preset identification pattern coincides with the midline of the second connection part of the joint corresponding to the preset identification pattern, and the midline of the second connection part of the joint corresponding to the preset identification pattern coincides with the axis line of the binocular camera, then the spatial coordinates of the top of the preset identification pattern are calculated based on the first image and the second image. , the lowest spatial coordinate , the leftmost spatial coordinate and the rightmost spatial coordinate ; By the uppermost spatial coordinate and the spatial coordinates of the bottom , calculate the center coordinates of the first marker , and move the first identification center coordinates by a distance r in the direction of the normal vector of the preset identification pattern to obtain the center coordinates of the joint corresponding to the preset identification pattern, wherein the direction of the normal vector is perpendicular from the center of the preset identification pattern to the central axis of the second connection part, and r is the outer diameter of the cross section of the second connection part; or, By the leftmost spatial coordinate and the rightmost spatial coordinates , calculate the center coordinates of the second marker , and move the second identification center coordinates by a distance r along the normal vector direction of the preset identification pattern to obtain the center coordinates of the joint corresponding to the preset identification pattern; or, The first mark center coordinates and the second mark center coordinates are averaged to obtain the third mark center coordinates. And move the third identification center coordinates by a distance r along the normal vector direction of the preset identification pattern to obtain the center coordinates of the joint corresponding to the preset identification pattern.

3. The binocular vision-based coaxial rope-driven manipulator configuration detection method according to claim 1 is characterized in that: The step of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result comprises: If the axis line of the preset identification pattern and the midline of the second connection part of the joint corresponding to the preset identification pattern do not coincide, but the midline of the second connection part of the joint corresponding to the preset identification pattern coincides with the axis of the binocular camera, then the coordinates of the upper end point of the bottom arc segment of the second connection part of the joint are obtained. , the coordinates of the lower endpoint , and the coordinates of the arc center point of the bottom arc segment ; Calculate the coordinates of the intersection of the first perpendicular bisector of segment AM and the second perpendicular bisector of segment BM , wherein the line segment AM is formed by connecting the upper endpoint and the center point of the arc, and the line segment BM is formed by connecting the lower endpoint and the center point of the arc; The intersection coordinates Along the central axis of the second connection part, and in the direction from the bottom surface of the second connection part of the same joint to the top surface of the second connection part, the moving distance , obtain the center coordinates of the joint, where h is the generatrix length of the second connecting part.

4. The binocular vision-based coaxial rope-driven manipulator configuration detection method according to claim 1 is characterized in that: The step of calculating the center coordinates of the joints corresponding to the preset identification patterns according to the judgment result comprises: If the axis line of the preset identification pattern and the midline of the second connection part of the joint corresponding to the preset identification pattern do not coincide, and the midline of the second connection part of the joint corresponding to the preset identification pattern and the axis of the binocular camera do not coincide, then obtain the coordinates of the upper end point of the bottom arc segment of the second connection part of the joint , the coordinates of the lower end point of the bottom arc segment , and the coordinates of the arc center point of the bottom arc segment ; Calculate the coordinates of the intersection of the first perpendicular bisector of segment AM and the second perpendicular bisector of segment BM , wherein the line segment AM is formed by connecting the upper endpoint and the center point of the arc, and the line segment BM is formed by connecting the lower endpoint and the center point of the arc; Get the leftmost spatial coordinates of the preset logo graphic and the rightmost spatial coordinate , calculate the vector , the vector Parallel to the central axis of the two connecting parts, and the first connecting part is located to the left of the second connecting part; According to the intersection coordinates and the vector , find the direction vector of the central axis of the second connecting portion and convert it into a central axis straight line expression; and the spatial coordinates of the top of the preset logo graphic , the lowest spatial coordinate , the leftmost spatial coordinate and the rightmost spatial coordinate , calculate the center of the circle passing through the preset identification pattern and perpendicular to and The normal vector of the determined plane, where , , and convert the normal vector into a normal line expression; According to the central axis straight line expression and the normal line straight line expression, the intersection coordinates of the central axis and the normal line are calculated, and the intersection coordinates are the center coordinates of the joint corresponding to the preset identification pattern.

5. The binocular vision-based coaxial rope-driven manipulator configuration detection method according to claim 1, characterized in that: The step of respectively calculating the center coordinates of each of the joints according to the first image and the second image comprises: Performing image preprocessing on the first image and the second image to obtain grayscale images; Extracting the edge of each of the joints from the grayscale image; Draw a circumscribed rectangle circumscribing the edges of the joint; The centroid coordinates of the circumscribed rectangle are calculated, and the centroid coordinates are used as the center coordinates of the joint.

6. The binocular vision-based coaxial rope-driven manipulator configuration detection method according to claim 1 is characterized in that: The step of determining the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints further includes: The spatial position of each joint is determined according to the central coordinates of each joint and the central axis of the second connection part of each joint.

7. The binocular vision-based coaxial rope-driven manipulator configuration detection method according to claim 6 is characterized in that: After the step of determining the spatial position of each joint according to the central coordinates of each joint and the central axis of the second connecting portion of each joint, the method further includes: According to the spatial posture of each joint, the configuration of the coaxial rope-driven robotic arm is presented in a three-dimensional diagram.

8. A coaxial rope-driven manipulator configuration detection device based on binocular vision, characterized in that: The coaxial rope-driven manipulator comprises a plurality of joints, and the device comprises: An acquisition module, used for acquiring a first image and a second image of the coaxial rope-driven manipulator taken by a binocular camera; A center coordinate calculation module, used to calculate the center coordinates of each of the joints according to the first image and the second image; A configuration determination module, used to determine the configuration of the coaxial rope-driven manipulator according to the central coordinates of each of the joints; Wherein, the joint comprises a first connection portion and a second connection portion connected to each other, the second connection portion comprises a top surface and a bottom surface opposite to each other, the top surface is a surface adjacent to the first connection portion of the same joint, and the bottom surface is a surface adjacent to another joint; Two adjacent joints are connected by inserting a first connection part of one joint into a second connection part of another joint; a plurality of the joints are connected in sequence to form the coaxial rope-driven robotic arm; The second connecting portion is cylindrical, and a preset identification pattern is provided on the outer side surface of the second connecting portion; The binocular camera is arranged at a preset distance from the side of the coaxial rope-driven robotic arm, and the side of the coaxial rope-driven robotic arm is a side where the binocular camera can capture the preset identification pattern; Among them, the preset identification graphic is a circle, and the center coordinate calculation module is used to determine, based on the first image and the second image, whether the axis line of each preset identification graphic and the midline of the second connection part of the joint corresponding to each preset identification graphic coincide with each other, and determine whether the midline of the second connection part of the joint corresponding to each preset identification graphic coincides with the axis of the binocular camera; based on the judgment result, calculate the center coordinates of the joint corresponding to each preset identification graphic.

Images