WO2022012337A1 - Moving arm system and control method - Google Patents

Abstract

The present disclosure relates to the field of robotic equipment. Disclosed is a control method for a moving arm. The control method comprises: obtaining an image collected by an image collection device, wherein the image comprises an image of a marker configured on the tail end of a moving arm, and the marker comprises a plurality of marker corner points; identifying the marker corner points in the collected image; and on the basis of the identified marker corner points, determining the current relative posture of the tail end of the moving arm relative to the image collection device. On the basis of a closed-loop control method for visual tracking, and a configured marker, the posture of the tail end of a moving arm is detected in real time, thereby effectively reducing the trajectory tracking error of the moving arm, such that the movement control error can be reduced.

Description

Motion arm system and control method

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims the priority of the Chinese patent application filed on July 11, 2020, the application number is 202010665741.7, and the invention name is "a control method, device, robot system and storage medium for a moving arm system", the full text of the application Incorporated herein by reference in its entirety.

technical field

The present disclosure relates to the field of robots, and in particular, to a motion arm system and a control method.

Background technique

The closed-loop in-position correction method adopted by the existing moving arm control method of the robot system mainly determines the compensation condition of the closed-loop compensation by acquiring the correction data from the in-position condition of the moving joint of the moving arm. This compensation method cannot solve the in-position accuracy problem caused by the error of the equipment itself, especially for the precise closed-loop control of the moving arm.

Existing spatial positioning devices usually use the identification of existing positioning marks to determine the spatial position of the object where the positioning marks are located. At present, there are many schemes for realizing such identification and control methods, but each has its own limitations. For example, a binocular camera is required for the spatial positioning of the marked ball, which requires more equipment. There is also a scheme that uses markers with contrast of light and dark or black and white as markers, and uses marker likelihood calculation for each pixel in the image, or uses an existing template to perform template matching on all pixels within a pixel within a certain range. This method is easy to misidentify the part that is not a marker as a marker, or has a very large amount of calculation, which is not suitable for the application of a high-precision real-time closed-loop correction algorithm.

To sum up, there are still many issues and technologies in the field of closed-loop control of moving arms that need to be addressed urgently.

SUMMARY OF THE INVENTION

In some embodiments, the present disclosure provides a control method of a moving arm, comprising: obtaining an image captured by an image acquisition device, the image including an image of a marker disposed on the end of the moving arm, the marker comprising a plurality of marker corners; identifying marker corners in the captured image; and determining the current relative position of the end of the moving arm relative to the image capturing device based on the identified marker corners posture.

In some embodiments, the present disclosure provides a motion arm system, comprising: an image acquisition device for acquiring an image; at least one motion arm, including a motion arm end with a marker on the motion arm end, the motion arm The marker includes a plurality of marker corners; and a control device configured to perform a control method according to some embodiments of the present disclosure.

In some embodiments, the present disclosure provides a computer-readable storage medium comprising one or more computer-executable instructions stored thereon, the computer-executable instructions being executed by a processor that has been configured to perform according to Control methods of some embodiments of the present disclosure.

Description of drawings

In order to clearly illustrate the technical solutions in the embodiments of the present disclosure, the accompanying drawings that are required to be used in the description of the embodiments of the present disclosure will be briefly introduced below. The accompanying drawings in the following description only show some embodiments of the present disclosure. For those of ordinary skill in the art, without any creative effort, others can also be obtained according to the contents of the embodiments of the present disclosure and these drawings. Example.

FIG. 1 shows a schematic structural diagram of a moving arm system according to some embodiments of the present disclosure;

Figure 2(a) shows a schematic structural diagram of a moving arm according to some embodiments of the present disclosure;

Figure 2(b) shows a schematic structural diagram of a marker according to some embodiments of the present disclosure;

FIG. 3 shows an enlarged schematic view of the end of the moving arm in a moving state according to some embodiments of the present disclosure;

FIG. 4 shows a flowchart of a control method of a moving arm system according to some embodiments of the present disclosure;

5 shows a flowchart of a control method for identifying marker corners in an image according to some embodiments of the present disclosure;

FIG. 6 shows a schematic diagram of corner search for suspected markers according to some embodiments of the present disclosure;

Figure 7 shows a flowchart of a method for determining the position of an end of a moving arm in a world coordinate system in accordance with some embodiments of the present disclosure;

8 shows a schematic structural diagram of a continuum flexible arm according to some embodiments of the present disclosure;

9 shows a schematic structural diagram of a single continuum segment of a continuum flexible arm according to some embodiments of the present disclosure.

detailed description

In order to make the technical problems solved by the present disclosure, the technical solutions adopted and the technical effects achieved more clearly, the technical solutions of the embodiments of the present disclosure will be described in further detail below with reference to the accompanying drawings. Obviously, the described embodiments are only the present disclosure. Exemplary embodiments, but not all embodiments.

In the description of the present disclosure, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. The indicated orientation or positional relationship is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present disclosure and simplifying the description, rather than indicating or implying that the indicated device or element must have a specific orientation or a specific orientation. construction and operation, and therefore should not be construed as limiting the present disclosure. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed to indicate or imply relative importance. In the description of the present disclosure, it should be noted that, unless otherwise expressly specified and limited, the terms "installed", "connected", "connected" and "coupled" should be understood in a broad sense, for example, it may be a fixed connection, or It can be a detachable connection; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium; it can be an internal connection between two components. For those of ordinary skill in the art, the specific meanings of the above terms in the present disclosure can be understood according to specific situations. In the present disclosure, in the surgical robot system, the end close to the user (such as a doctor) is defined as proximal, proximal or rear, rear, and the end close to the surgical patient is defined as distal, distal or front, anterior.

In this disclosure, the term "position" refers to the location of an object or a portion of an object in three-dimensional space (eg, the three translational degrees of freedom can be described using changes in Cartesian X, Y, and Z coordinates, such as along the Cartesian X, respectively axis, three translational degrees of freedom in Y-axis and Z-axis). In this disclosure, the term "pose" refers to a rotational setting of an object or a portion of an object (eg, three rotational degrees of freedom, which may be described using roll, pitch, and yaw). In the present disclosure, the term "pose" refers to a combination of position and pose of an object or a part of an object, which can be described, for example, using the six parameters of the six degrees of freedom mentioned above. In the present disclosure, the pose of the moving arm or a part thereof refers to the pose of the coordinate system defined by the moving arm or a part thereof relative to the coordinate system defined by the support, the base where the moving arm is located, or the world coordinate system.

FIG. 1 shows a structural block diagram of a motion arm system 100 according to some embodiments of the present disclosure. As shown in FIG. 1 , the moving arm system 100 may include an image capturing device 10 , at least one moving arm 20 and a control device 30 . The image capturing device 10 and the at least one moving arm 20 are respectively connected in communication with the control device 30 . In some embodiments, as shown in FIG. 1 , the control device 30 may be used to control the movement of the at least one moving arm 20 to adjust the posture, coordinate with each other, and the like of the at least one moving arm 20 . In some embodiments, at least one kinematic arm 20 may include a kinematic arm tip 21 at the tip or distal end. In some embodiments, at least one movement arm 20 may further include a movement arm body 22 , and the movement arm end 21 may be a distal end portion of the movement arm body 22 or an end effector disposed at the distal end of the movement arm body 22 . The control device 30 can control the movement of the at least one moving arm 20 to move the end 21 of the moving arm to a desired position and posture. Those skilled in the art should understand that the motion arm system 100 can be applied to a surgical robotic system, such as a laparoscopic surgical robotic system. It should be understood that the kinematic arm system 100 may also be applied to special purpose or general purpose robotic systems in other fields (eg, manufacturing, machinery, etc.).

In the present disclosure, the control device 30 may be connected in communication with the motor of the at least one moving arm 20, so that the motor controls the at least one moving arm 20 to move to a corresponding target pose based on the driving signal. For example, the motor that controls the movement of the moving arm can be a servo motor, which can be instructed by the control device to control the movement of the moving arm. The control device 30 can also be connected in communication with a sensor coupled to the motor, for example, through a communication interface, so as to receive movement data of the moving arm 20 and monitor the movement state of the moving arm 20 . In an example of the present disclosure, the communication interface may be a CAN (Controller Area Network) bus communication interface, which enables the control device 30 to communicate with the motor and the sensor through the CAN bus.

In some embodiments, the moving arm 20 may comprise a continuous flexible arm or a moving arm composed of multiple joints with multiple degrees of freedom, for example, a moving arm that can achieve 6 degrees of freedom motion. The image capture device 10 may include, but is not limited to, a dual-lens image capture device or a single-lens image capture device, such as a binocular or monocular camera.

FIG. 2( a ) shows a schematic structural diagram of a moving arm 20 according to some embodiments of the present disclosure. As shown in FIG. 2( a ), the moving arm 20 may include a moving arm end 21 and a moving arm body 22 , and a marker 211 is fixed on the moving arm end 21 . Figure 2(b) shows a schematic structural diagram of the marker 211 according to some embodiments of the present disclosure. As shown in FIG. 2( b ), the marker 211 may include a plurality of marker corners 2111 . In some embodiments, the marker 211 can be in the shape of a cylinder, and is fixedly covered on the end 21 of the moving arm. The marker 211 may include a plurality of marker corners 2111 distributed on the cylindrical shape (eg, regularly distributed). It should be understood that the markers may be graphics in known arrangements with distinct differences in brightness, grayscale, and hue. The corners of the marker can be points with special distinguishing features on the marker, and the graphics or colors in the surrounding range of the corners of the marker can be point-symmetrically distributed, so that the image acquisition device can collect the corners of the marker. In some embodiments, the plurality of marker corner points are distributed regularly, which facilitates the determination of parameters of the marker corner points, such as parameters such as distribution angle and separation distance between the plurality of marker corner points. In some embodiments, as shown in FIG. 2( b ), the markers 211 may include, but are not limited to, black and white checkerboard markers, and the corner points 2111 of the markers may be, for example, intersections of two line segments in the checkerboard marker .

FIG. 3 shows an enlarged schematic view of the end of the moving arm in a moving state according to some embodiments of the present disclosure. As shown in FIG. 3 , the at least one moving arm may include, but is not limited to, two moving arms, each of which is provided with a marker at the end.

Some embodiments of the present disclosure provide a control method of a moving arm. FIG. 4 shows a flowchart of a control method 400 of a motion arm system (eg, motion arm system 100 ) according to some embodiments of the present disclosure. As shown in FIG. 4 , the method 400 may be performed by a control device (eg, the control device 30 ) of the motion arm system 100 . The control device 30 may be configured on a computing device. Method 400 may be implemented by software, firmware, and/or hardware.

In step 401, an image captured by an image capturing device is obtained. For example, an image of the end of the moving arm may be captured by a dual-lens image capturing device or a single-lens image capturing device, and the captured images include images of markers on the end of the moving arm.

At step 403, marker corners in the acquired image are identified. In some embodiments, the acquired images may be pre-processed by the control device to identify marker corners in the images. An exemplary method of identifying marker corners in an image is detailed in the method shown in FIG. 5 .

At step 405, based on the identified marker corners, the current relative pose of the end of the moving arm relative to the image capture device is determined. In some embodiments, the current pose of the marker relative to the image acquisition device may be determined based on the identified corner points of the marker, and based on the current pose of the marker and the relative pose of the marker relative to the end of the moving arm, the determination may be made The current relative pose of the end of the kinematic arm. It should be understood that since the marker is fixedly arranged on the end of the moving arm, the relative pose of the marker with respect to the end of the moving arm is known.

5 shows a flowchart of a method 500 for identifying marker corners in acquired images, according to some embodiments of the present disclosure. As shown in FIG. 5 , the method 500 may be performed by a control device (eg, control device 30 ) of a moving arm system (eg, moving arm system 100 ). The control device 30 may be configured on a computing device. Method 500 may be implemented by software, firmware, and/or hardware.

In step 501, a region of interest (ROI) is determined in the acquired image. In some embodiments, the ROI is determined to be a full image, or the ROI is determined to be a partial image based on the position of suspected marker corner points in a previous frame of image (eg, the image processed in the previous motion control loop), and the ROI is determined as a partial image. Convert to the corresponding grayscale image. For example, based on the collected image, the whole image or partial image can be intercepted as the ROI, and the ROI can be converted into a corresponding grayscale image to quantify the grayscale information of each pixel. In some embodiments, it may be determined that the ROI of the first frame of image is the full image based on the captured image being the first frame of image. In some embodiments, based on the acquired image being not the first frame image, the ROI may be determined as a partial image based on the position of the corner point of the suspected marker in the previous frame image.

In step 503, suspected marker corners are identified in the ROI. For example, the possibility that each pixel is a corner of the marker can be determined based on the grayscale information of each pixel, and a pixel with a high probability is determined as a corner of the suspected marker.

In some embodiments, the partial image may include a set distance range centered on a virtual point formed by the average coordinates of the corner points of the suspected marker in the previous frame of images, and the set distance may include the average interval of the corner points of the multiple suspected markers A predetermined multiple of the distance. It should be understood that, for example, the predetermined multiple may include, but is not limited to, a fixed multiple, such as twice, of the average separation distance between the corner points of a plurality of suspected markers. It should be understood that the predetermined multiple can also be a variable multiple of the average separation distance between the corner points of a plurality of suspected markers.

In some embodiments, a corner likelihood (CL) for each pixel in the ROI can be determined. Divide the ROI into multiple sub-ROIs, and determine the pixel with the largest CL value in each sub-ROI. Based on a plurality of pixel points with the largest CL values of the plurality of sub-ROIs, a set of pixel points with a CL value greater than a first threshold is determined. Determine the pixel point with the largest CL value in the pixel point set as the corner point of the first suspected marker. Based on the first suspected marker corner points, the second suspected marker corner points are searched along a plurality of edge directions with a set step size.

In some embodiments, a corner likelihood (CL) is determined for each pixel in the ROI. For example, a convolution operation is performed on each pixel in the range of the ROI image to obtain the first-order and/or second-order derivative of each pixel. The corner likelihood (CL) of each pixel is obtained by using the first and/or second derivative of each pixel in the aforementioned ROI image range.

In some embodiments, the CL of each pixel in the ROI image can be calculated according to the following formula:

CL=max(c _xy ,c ₄₅ )

c _xy =ε ² ·|I _xy |-1.5 ·ε ·(|I ₄₅ |+|I _n45 |)

c ₄₅ =ε ² ·|I _{45_45} |-1.5 ·ε ·(|I _x |+|I _y |) (1)

Among them, ε is a set constant, I _x , I ₄₅ , I _y , and I _n45 are the first-order derivatives of each pixel in the four directions of 0, π/4, π/2 and -π/4; I _xy and I _{45_45} , which are the second derivative of each pixel in the 0, π/2 and π/4, -π/4 directions, respectively; c _xy is the CL value of each pixel in the 0, π/2 direction, c ₄₅ is the CL value of each pixel in the π/4 direction.

In some embodiments, the ROI is divided into multiple sub-ROIs. For example, the non-maximum suppression method can be used to evenly segment multiple sub-ROIs in an ROI image range. In some embodiments, the ROI image may be equally divided into multiple sub-ROIs of 5x5 pixels. The above-mentioned embodiments are exemplary and non-limiting, and it should be understood that the ROI image may also be divided into multiple sub-ROIs of other sizes. The pixel point with the largest CL value in each sub-ROI may be determined, and the pixel point with the largest CL value in each sub-ROI is compared with the first threshold to determine a set of pixel points whose CL value is greater than the first threshold. In some embodiments, the first threshold may be set to 0.06. It should be understood that the first threshold value can also be set to other values.

In some embodiments, the pixel point with the largest CL value in the pixel point set is determined as the corner point of the first suspected marker. For example, all the pixels in the pixel set may be sorted in descending order of CL value, and the pixel with the largest CL value may be used as the first suspected marker corner. Based on the first suspected marker corner points, a second suspected marker corner point is searched along a plurality of edge directions with a set step size. FIG. 6 shows a schematic diagram of corner search for suspected markers according to some embodiments of the present disclosure. As shown in FIG. 6 , starting from the corner point of the first suspected marker, the second suspected marker corner points are sequentially searched for the four edge directions with a set step size. In some embodiments, the set step size may be 10 pixels. For example, taking the corner of the first suspected marker as the starting point and taking 10 pixels as the step, the pixel with the largest CL value is sequentially searched in the set range in the edge direction as the second corner of the suspected marker. For example, the setting range may be a 10×10 square area. It should be understood that the above-mentioned embodiment is an example, not a limitation, the set step size may also be other number of pixels, and the set range may also be an area of other size. For example, four edge directions of the corner points of the first suspected marker can be determined, and based on each edge direction, a mobile search is performed with a set step size to find the pixel with the largest CL value within the set range, as the second suspected point. Marker corners.

In some embodiments, by determining the gradient direction and gradient weight of each pixel point in the first predetermined range centered on the corner of the first suspected marker, based on the gradient direction and the gradient direction of each pixel point in the first predetermined range Gradient weights to determine multiple edge directions. For example, the first predetermined range may be a 10×10 square area. In a 10×10 square pixel neighborhood centered on the corner of the first suspected marker, determine the gradient direction and gradient weight of each pixel to determine multiple edge directions of the corner of the first suspected marker. It should be understood that the corner point of the second suspected marker can also determine the edge direction by a similar method. By adjusting the edge direction based on the corners of the suspected markers, the corners of the markers can be searched in a targeted manner to reduce the amount of computation.

In some embodiments, the gradient direction and gradient weight of each pixel in the first predetermined range can be calculated by the following formula:

Wherein, I _angle is the gradient direction _{of the corresponding pixel, and I weight} is the gradient weight of the corresponding pixel; I _x and I _y are the first-order derivatives of the corresponding pixel in the 0 and π/2 directions, respectively. _{The I angle} and I _weight of each pixel in the first predetermined range are calculated by a clustering method to obtain the edge direction of the corresponding pixel.

In some embodiments, the edge direction may be adjusted based on the second suspected marker corner, and along the adjusted edge direction, the third suspected marker corner may be searched. For example, similar to the above, by determining the gradient direction and gradient weight of each pixel point in the predetermined range centered on the corner of the second suspected marker, and based on the gradient direction and gradient weight of each pixel point in the predetermined range, determine The updated edge orientation, as shown in Figure 6.

In step 505, based on the standard corner model, a correlation test is performed on the corners of the suspected markers in the ROI to determine whether the corners of the suspected markers in the ROI image are marker corners. For example, the pixel points of the first suspected marker corner point and the searched suspected marker corner point (for example, the second suspected marker corner point) can be substituted into the standard corner point model for correlation judgment, so as to determine the suspected marker corner point Whether it is a marker corner. The corner detection method can be based on the combination of corner likelihood estimation (Corner Likelihood) and template matching (Template Matching) to determine the image (for example, when the image acquisition device is a binocular image acquisition device, the extracted image includes the left lens image and the right lens image. marker corners in the lens image).

In some embodiments, a correlation coefficient between the grayscale distribution of the standard corner model and the grayscale distribution of pixels in a second predetermined range centered on the pixel of the suspected marker corner is determined, in response to the correlation coefficient being greater than The third threshold is to determine the corner of the suspected marker as the corner of the marker. In some embodiments, the marker may be a black and white checkerboard pattern, and the standard corner model may be a hyperbolic tangent model (HTM). For example, the suspected marker angle can be determined based on the correlation coefficient CC between the gray distribution of a standard corner model (eg, HTM) and the gray distribution of a predetermined range of pixel neighborhoods centered on the pixel of the suspected marker corner. Whether the point is a marker corner. For example, the second predetermined range centered on the pixel point of the suspected marker corner point may include, but is not limited to, a 10×10 square pixel neighborhood. The correlation coefficient CC can be defined as follows:

Among them, G _image is the gray distribution of the pixel neighborhood in a predetermined range centered on the pixel of the suspected marker corner, G _HTM is the gray distribution of the standard corner model, Var is the variance function, and Cov is the covariance function.

When judging that the correlation coefficient CC value is greater than the threshold, it means that the grayscale distribution in the predetermined range centered on the pixel has a high correlation with the hyperbolic tangent model, and the suspected marker corner can be determined to be the marker corner. On the contrary, it is determined that the suspected marker corner is not a marker corner. In some embodiments, the threshold may be set to 0.8. It should be understood that the threshold value may also be other set values. For example, take the corner point of the first suspected marker as the starting point, search for the corner point of the second suspected marker, perform correlation verification on the corner point of the second suspected marker that has been searched, determine that the value of the correlation coefficient CC is greater than 0.8, and determine the second corner point of the suspected marker. Suspected marker corners are marker corners.

It should be understood that after each search for a suspected marker corner point or after multiple suspected marker corner points are searched, the corresponding pixel points may be substituted into the standard corner point model for correlation judgment to determine the identified suspected marker corner points. Whether the point is a marker corner.

In some embodiments, based on the first suspected marker corner point and the second suspected marker corner point, searching in opposite edge directions may be performed to determine other suspected marker corner points. As shown in FIG. 6 , starting from the corner point of the first suspected marker and the corner point of the second suspected marker, searches are performed in two opposite edge directions respectively. For example, the first suspected marker corner point is used as a starting point to search in a direction away from the second suspected marker corner point, and the second suspected marker corner point is used as a starting point to search in a direction away from the first suspected marker corner point.

In some embodiments, the search may be stopped in response to the number of suspected marker corners being greater than or equal to the second threshold. In some embodiments, the second threshold may be set to four. It should be understood that the above embodiment is an example, not a limitation, and the second threshold may also be set to 5 or other values. For example, it is determined that the total number of suspected marker corner points is greater than or equal to 4 and the correlation verification is passed, indicating that the marker is successfully found, and the determined multiple suspected marker corner points are used as marker corner points.

In some embodiments, in response to the searched distance being greater than a predetermined multiple of the distance between the first suspected marker corner point and the second suspected marker corner point or in response to the determined number of suspected marker corner points being less than the second threshold, Based on the next pixel in the pixel point set in descending order of CL value as the first suspected marker corner, search for suspected marker corners along the edge direction with a set step size until the number of suspected marker corners is greater than or equal to equal to the second threshold. In some embodiments, the predetermined multiple may be twice the distance between the first suspected marker corner point and the second suspected marker corner point, the search distance is greater than the predetermined multiple, and the current search cycle ends. The next search cycle can be restarted based on the next pixel point in the pixel point set in descending order of the CL value as the first suspected marker corner point. Or, based on the above-mentioned search, it is determined that the total number of corner points of the suspected marker is less than 4, indicating that the marker has not been successfully identified, and the next pixel in the pixel point set in descending order of the CL value is used as the first suspected marker. Corner, start the next search cycle, search for other suspected marker corners, until the total number of suspected marker corners is determined to be greater than or equal to a second threshold (eg, 4) and the correlation is verified.

In some embodiments, sub-pixel localization of the marker corner is performed in response to the suspected marker corner in the ROI being the marker corner. The coordinate accuracy of marker corners can be optimized by sub-pixel positioning. In some embodiments, the CL value of each pixel in each sub-ROI can be fitted based on a model to determine the coordinates of the sub-pixel positioned marker corners. For example, the fitting function of the CL value of each pixel point in each sub-ROI may be a quadratic surface function, and the extreme point of the function is a sub-pixel point. The fitting function can be as follows:

S(x,y)=ax ² +by ² +cx+dy+exy+f

(4)

Among them, S(x, y) is the CL value fitting function of all pixels in each sub-ROI, and a, b, c, d, e, and f are coefficients;

where x _c is the x coordinate _{of the corner of the marker, and y c} is the y coordinate of the corner of the marker.

In some embodiments, the three-dimensional coordinates of the marker corner points relative to the marker coordinate system may be determined, the two-dimensional coordinates of the marker corner points in the acquired image may be determined, and the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates may be determined. The analytical solution, based on the analytical solution of the homography matrix, determines the current relative pose of the marker relative to the image acquisition device. It will be appreciated that the three-dimensional coordinates of the image-identified marker corners may be determined based on known parameters of the marker corners. It should be understood that, based on the dual-lens image acquisition device, the following processing may be performed after aligning the corners of markers in the left-lens image and the right-lens image. For example, the three-dimensional coordinates of the marker corners relative to the marker coordinate system can be as follows:

in,

are the three-dimensional coordinates corresponding to the corner point of the ith marker, r _wm and β may be parameters determined based on the known characteristics of the marker. For example, for the black and white checkerboard markers, which are arranged as cylinders at the end of the moving arm, the corner points of the checkerboard markers are evenly distributed around the circumference of the cylinder markers (as shown in Figure 2(b)), where r _wm is The radius of the cylinder formed by the marker, and β is the distribution angle of the corner points of the marker.

In some embodiments, based on the single-lens image acquisition device, the corner points of the marker can be calculated directly based on the parameters determined by the known characteristics of the marker (such as the radius of the formed cylinder and the distribution angle of the corner points of the marker). The three-dimensional coordinates of the object coordinate system. Based on the dual-lens image acquisition device, the corner points of the markers detected in the left and right images can be aligned, so that the coordinates of the same space point in the y direction after binocular stereo correction are consistent, and then the parameters determined based on the known features of the markers , calculate the three-dimensional coordinates of the marker corners in the marker coordinate system. The marker coordinate system can be established as follows:

Wherein, the origin of the marker coordinate system coincides with the marker center, and the x-axis may point from the marker center to the first marker corner point (for example, the first determined marker corner point).

In some embodiments, determining the analytical solution of the homography matrix of the two-dimensional and three-dimensional coordinates may include calculating based on the following formula:

H _left =[h ₁ h ₂ h ₃ ]=η·A·[r ₁ r ₂ ^ll p _wm ] (8)

In formula (7),

Coordinates for the expansion of the i-th marker corner point, [u _i, v _i] is the i th corner marker two-dimensional coordinates, ^left s _i is an arbitrary nonzero scalar, ^left s _i is the scale factor, to Make the numbers in the last column on the left and right sides of equation (7) the same, and H _left is a homography matrix;

In formula (8), A is the known internal parameter matrix of the image acquisition device, r ₁ , r ₂ are the first two columns of the ^ll R _wm matrix, ll represents the image acquisition device coordinate system {ll} (here, the left lens coordinate system For example), n is any non-zero scalar. It will be appreciated that by introducing extended coordinates, the calculation process can be simplified.

In some embodiments, the analytical solution of the homography matrix may also include calculation based on the following formula:

Among them, x _H =[h ₁ ^T h ₂ ^T h ₃ ^T ] ^T , L is a 2n×9 matrix, n is the number of marker corners determined in the acquired image, and x _H is the singular value decomposition of matrix L (SVD ) is the right singular vector corresponding to the smallest singular value after .

In some embodiments, the current relative pose of the marker relative to the image acquisition device can be determined based on the following formula:

r ₁ =κ·A ⁻¹ ·h ₁

r ₂ =κ·A ⁻¹ ·h ₂

^ll R _wm =[r ₁ r ₂ r ₁ ×r ₂ ]

^ll p _wm =κ·A ^-1 h ₃ (10)

Here, κ=1/||A ⁻¹ ·h ₁ ||=1/||A ⁻¹ ·h ₂ ||.

In some embodiments, a nonlinear optimization process may be performed to obtain the current relative position of the marker with respect to the image acquisition device based on an analytical solution of the homography matrix between the two-dimensional and three-dimensional coordinates of each marker corner point posture. For example, a nonlinear optimization process is established to calculate the current relative pose of the marker with respect to the image acquisition device by algebraic methods. For example, the nonlinear optimization process may include minimizing the average geometric error between the projection of the three-dimensional coordinates on the acquired image and the marker corners.

In some embodiments, the image capturing device is a single-lens image capturing device, and the captured image is a single-lens (herein, the left lens is taken as an example, denoted as "left") images, and the nonlinear optimization process may include optimization based on the following formula:

In some embodiments, the image capturing device is a dual-lens image capturing device, and the captured images include a left-lens (denoted as "left") image and a right-lens (denoted as "right") image, and the nonlinear optimization process may include the following: Formula optimization:

Among them, ^ll R _wm and ^ll p _wm are respectively the pose and position of the marker in the coordinate system of the image acquisition device (or the left lens coordinate system) {11}; H _left and H _right are the image with the left lens and the right lens, respectively image-related homography matrices; ^left s _i and ^right s _i , respectively, are the scale factors for the analytical decorrelation of the homography matrices related to the left-lens image and the right-lens image, respectively,

and

are the extended coordinates of the corner point of the i-th marker related to the left-lens image and the right-lens image, respectively. It should be understood that H _left is the homography matrix corresponding to the two-dimensional coordinates and three-dimensional coordinates of the marker corners _{captured by the left camera image, and H right} is the corresponding two-dimensional coordinates and three-dimensional coordinates of the marker corners captured by the right camera image. ^Homography matrix, left s _i is a non-zero scalar associated with the analytical solution of _{H left} ^{, and right} s _{i is} a non-zero scalar associated with the analytical solution of H _right. For example, when the image acquisition device is a dual-lens image acquisition device, a nonlinear optimization process can be established through the correspondence between the two-dimensional coordinates and the three-dimensional coordinates of the marker corners in the left-lens image and the right-lens image. The pose of the marker relative to the coordinate system of the image acquisition device [ ^ll R _wm ^ll p _wm ] is the optimization variable, and the corner points of the three-dimensional marker are reprojected to the left lens image and the marker corner on the right lens image and the actual detection mark The minimum average geometric error between the object corners is taken as the optimization objective to obtain the optimal left-lens extrinsic parameters (such as the relative pose of the marker and the dual-lens image acquisition device). It should be understood that when the right lens in the dual-lens image acquisition device is used as the main lens, the left and right mirrors are adjusted, and the algorithm is basically similar to the aforementioned process, and will not be described in detail in this disclosure.

By identifying the corner points of the marker in the image captured by the image acquisition device, the pose matrix [ ^ll R _wm ^ll p _wm ] of the marker relative to the image acquisition device is optimized and estimated, and the end of the moving arm relative to the world coordinate system is calculated based on the formula The pose, and based on the pose information of the end of the moving arm, the real-time closed-loop control of the pose of the end of the moving arm is performed to improve the motion accuracy of the moving arm of the system.

Figure 7 shows a flowchart of a method 700 for determining the position of the end of a moving arm in a world coordinate system, according to some embodiments of the present disclosure. As shown in FIG. 7 , the method 700 may be performed by a control device (eg, control device 10 ) of a moving arm system (eg, moving arm system 100 ). The control device 30 may be configured on a computing device. Method 700 may be implemented by software, firmware, and/or hardware.

In step 701, the current pose of the moving arm in the world coordinate system is determined based on the current relative pose of the end of the moving arm. In some embodiments, the marker may be based on the identified corner, the marker is determined with respect to the image capture device current relative pose (e.g., position and posture ^ll R _wm ^ll p _wm). Based on the known relative pose relationship between the marker and the end of the moving arm, and the pose of the image acquisition device in the world coordinate system, the position of the end of the moving arm where the marker is located in the world coordinate system can be determined. For example, determining the position of the end of the kinematic arm in the world coordinate system can be based on the following formula:

^W p _tip = ^W R _ll ( ^ll R _wm ^wm p _tip + ^ll p _wm )+ ^W p _ll

(13)

Among them, ^W p _tip is the position of the end of the moving arm in the world coordinate system {W}, ^wm p _tip is the position of the end of the moving arm in the marker coordinate system {wm}; ^W R _ll and ^W p _ll are the image acquisition The pose and position of the device in the world coordinate system {W}; ^ll R _wm and ^ll p _wm are the pose and position of the marker in the image acquisition device coordinate system {ll}, respectively. It should be understood that the pose determination of the image acquisition device, ^W R ₁₁ and ^W p ₁₁ may be known matrices.

in,

is the position coordinate of the end of the moving arm in the marker coordinate system {wm}.

In this disclosure, the world coordinate system

Its origin can be located at the center of the fixed base (eg, sheath) (as shown in Figure 2(a) or Figure 8). marker coordinate system

Its origin coincides with the center of the movement arm marker, and the x-axis points from the center of the marker to the first marker corner (as shown in Figure 2(a), Figure 2(b)). Left camera coordinate system

Its origin coincides with the center of the left lens of the dual-lens image acquisition device, and the x-axis points from the center of the left lens of the dual-lens image acquisition device to the center of the right lens of the dual-lens image acquisition device. Right lens coordinate system

Its origin coincides with the center of the right lens of the dual-lens image acquisition device, and the x-axis coincides with the x-axis of the left lens coordinate system of the dual-lens image acquisition device, and points to the same.

In step 703, based on the target pose and the current pose of the end of the moving arm in the world coordinate system, the difference between the target pose and the current pose of the end of the moving arm is determined. In some embodiments, the target pose of the end of the moving arm in the world coordinate system may be input by the user through an input device. Through the comparison calculation, the difference between the target pose and the current pose of the end of the moving arm can be determined. For example, based on the target position and the current position of the end of the moving arm in the world coordinate system, the difference Δp between the target position and the current position can be calculated, and Δp can be as follows:

Δp=p _d -p _c

(14)

Where, p _d at the end of the movement arm to the target position in the world coordinate system, p _c is the current position of the movement arm in the end of the world coordinate system (e.g., process step 701 the calculated ^W p _tip).

Based on the difference Δp, the space velocity v and parameters of the end of the moving arm are obtained

They are:

v=v _xlim Δp/||Δp|| (15)

Among them, v _xlim is the set maximum allowable space velocity; J ⁺ is the Moore–Penrose pseudo-inverse matrix of J; ω is the angular velocity of the end of the moving arm; ψ can represent the bending angle of the moving arm or the rotation angle parameter vector of the joint of the moving arm ,

can be the derivative of ψ. For example, when the moving arm is a flexible moving arm, ψ is the bending angle parameter vector of the arm body of the flexible moving arm. When the moving arm is a common joint moving arm structure, ψ is the rotation angle parameter vector of the moving arm joint.

At each motion control loop, the parameter space vector at the end of the motion arm can be updated:

where Δt is the period of the motion control loop.

At step 705, a drive signal for the moving arm is determined based on the difference value and an inverse kinematics numerical iterative algorithm of the moving arm. For example, based on the difference between the target pose of the end of the moving arm in the world coordinate system and the current pose, through the inverse kinematics numerical iterative algorithm of the kinematics model of the moving arm, it can be determined that the joints included in the moving arm are currently moving The drive value within the control loop (or the drive value of the corresponding multiple motors that control the motion of the moving arm). It should be understood that the kinematic model may represent a mathematical model of the kinematic relationship of the joint space and the task space of the moving arm. For example, the kinematic model can be established by methods such as DH parameter method and exponential product representation.

In some embodiments, the method 700 may further include determining a drive signal for the motion arm at predetermined cycles to implement a plurality of motion control cycles. For example, multiple motion control loops are iteratively executed, and in each motion control loop, the control method according to some embodiments of the present disclosure may be executed to control the motion of the moving arm to the target pose. By iteratively executing multiple motion control loops, real-time closed-loop control of the end position of the motion arm can be achieved, which can improve the position control accuracy of the motion arm and avoid kinematic modeling caused by the open-loop control algorithm when the end of the motion arm has a load Inaccurate. It should be understood that, by implementing the position control of the moving arm through the method of the present disclosure, the trajectory tracking error of the moving arm (eg, the continuum flexible arm) can be improved. For example, in some embodiments, the trajectory tracking error of the moving arm can be reduced to 25.23% of the open loop control error.

In some embodiments, the kinematic arm is a flexible kinematic arm (eg, a continuum flexible arm) as an example. 8 and 9 respectively show a schematic structural diagram of a continuum flexible arm and a structural schematic diagram of a single continuum segment of the continuum flexible arm according to some embodiments of the present disclosure. As shown in Figures 8 and 9, the continuum flexible arm may include two continuous body segments and two straight rod segments, each continuous body segment (as shown in Figure 9) comprising 2 degrees of freedom structures, wherein one straight rod segment is located in Between the two continuums, the other straight rod segment contains 1 degree of freedom for feeding and 1 degree of freedom for the whole to rotate around its own axis.

As shown in FIG. 9, each continuum segment may include a base ring, an end ring, and a plurality of juxtaposed structural bones extending through the base ring and the end ring, and the plurality of structural bones may be fixedly connected to the end ring and to the base ring Slide to connect. The motion process of the continuum flexible arm is plane bending. The kinematics of the continuum flexible arm and the continuum segments it contains are described as follows:

In this disclosure, it should be understood that the base ring coordinate system

Attached to the base ring of the continuum segment of Section t, with its origin at the center of the base ring and the XY plane coincident with the base ring plane,

Point from the center of the base ring to the first structural bone.

Curved Plane Coordinate System

Its origin coincides with the origin of the base ring coordinate system, the XZ plane coincides with the bending plane,

and

coincide.

Curved Plane Coordinate System

Its origin is at the center of the end ring, the XY plane and the bending plane coincide,

and

coincide.

end ring coordinate system

Attached to the end ring of the t-th continuum segment with its origin at the center of the end ring and the XY plane coincident with the end ring plane,

Point from the center of the end ring to the first structural bone.

In some embodiments, the definitions of each symbol for kinematic modeling of the continuous flexible arm and the continuous body segment may be as shown in Table 1.

Table 1

As shown in Figure 9, the kinematics of a single continuum segment are as follows:

^{The position tb} p _tc and attitude ^tb R _te of the end of a single continuum segment can be expressed as the following formulas:

^tb R _te = ^tb R _t1 ^t1 R _t2 ^t2 R _te

(19)

_{The relationship between the angular velocity ω t} , the linear velocity v _t and the _{velocity of θ t} , δ _t at the end of a single continuum segment can be shown as the following formula:

In some embodiments, the kinematic description of the entire continuum of flexible arms is established as follows:

According to the transformation relationship between the coordinate systems shown in Figure 8, the end position and attitude of the flexible arm of the continuum can be expressed in the world coordinate system {w} as:

^W T _tip = ^W T _1b ^1b T _1e ^1e T _2b ^2b T _2e ^2e T _tip

(twenty one)

Among them, ^W T _tip represents the homogeneous transformation matrix of the end of the continuum relative to the world coordinate system; ^W T _1b represents the homogeneous transformation matrix of the base ring of the first continuum relative to the world coordinate system; ^1b T _1e represents the first Homogeneous transformation matrix of the end rings of one continuum relative to the base rings of the first continuum ^{; 1e} T _2b denotes the homogeneous transformation of the base rings of the second continuum relative to the end rings of the first continuum transformation matrix; ^2b T _2e represents the homogeneous transformation matrix of the end ring of the second continuum relative to the base ring of the second continuum; ^2e T _tip represents the end of the continuum relative to the end ring of the second continuum The homogeneous transformation matrix of .

It should be understood that the homogeneous transformation matrix of the end position of the continuum flexible arm in the world coordinate system {w} can be expressed as:

^W T _tip = ^W T _wm ^wm T _tip

(twenty two)

It should be understood that the velocity of the end of the entire continuum flexible arm can be expressed as the following formula:

in,

is the velocity Jacobian matrix; W ₁ , W ₂ and W _{3 in the} matrix can be expressed as the following formulas respectively:

W ₃ =J _2v

(26)

is the expression of the vector from coordinate system B to coordinate system C under coordinate system A;

is the antisymmetric matrix of the vectors.

In this Jacobian matrix:

Those skilled in the art can understand that, based on the inverse kinematics solution of the kinematics model of the continuum kinematic arm, for example, based on equations (15), (16) and (23), it is possible to determine the corresponding multiple motors that control the motion of the continuum kinematic arm Drive value within the current motion control loop.

In some embodiments, the present disclosure provides a computer-readable storage medium that can include at least one instruction executed by a processor to configure the processor to perform the control in any of the above embodiments method.

In some embodiments, the present disclosure provides a computer system that can include a non-volatile storage medium and at least one processor. The non-volatile storage medium may include at least one instruction. The processor is configured to execute at least one instruction to configure the processor to perform the control method in any of the above embodiments.

In some embodiments, a computer-readable storage medium may be a tangible device that can hold and store instructions for use by an instruction execution device. The computer-readable storage medium may be, for example, but not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any combination of the above.

In some embodiments, computer readable storage media may include, but are not limited to, portable computer disks, hard disks, read only memory (ROM), random access memory (RAM), erasable programmable read only memory (EPROM) , Electrically Erasable Programmable Read-Only Memory (EEPROM), Flash memory or other solid-state memory technology, CD-ROM, Digital Versatile Disc (DVD), HD-DVD, Blue-Ray or other optical storage devices, magnetic tape, Disk storage or other magnetic storage device, or any other medium capable of storing required information and accessible by a computer, having stored thereon computer-executable instructions that, when executed in a machine (eg, a computer device), The machine is caused to execute the control method of the present disclosure. It should be understood that computer devices may include personal computers, servers, or network devices, among others.

It will be understood that each process and block in the flowchart and block diagrams, and combinations of processes and blocks in the flowchart and block diagrams, can be implemented by computer-executable instructions. These computer-executable instructions may be provided to the processor of a general purpose computer, special purpose computer, embedded processor or other programmable data processing device to produce a machine such that the instructions executed by the processor of the computer or other programmable data processing device produce Means for implementing the functions specified in one or more of the flowcharts and one or more blocks of the block diagrams.

Some embodiments of the present disclosure can detect the posture of the end of the moving arm in real time based on the closed-loop control method of visual tracking and the set markers, which can effectively reduce the trajectory tracking error of the moving arm, and can reduce the motion control error. Furthermore, the control method according to some embodiments of the present disclosure may execute a motion control loop in a predetermined period, and in each motion control loop, determine the posture of the end of the moving arm, and control the motion of the moving arm to achieve the target posture.

Some embodiments of the present disclosure, based on a targeted marker corner search strategy, greatly reduce the amount of computation, and can iteratively implement trajectory tracking and error correction for the end of the moving arm.

In some embodiments of the present disclosure, by using different methods in different steps to identify and determine marker corners, misrecognition and miss of marker corners caused by various factors can be avoided, and the marker corners can be identified and missed in a short period of time. Achieve high-precision tracking and real-time correction of the end of the moving arm. And the technical solution of the present disclosure has a wide range of adaptability, can be applied to a variety of markers and marker identification equipment, and can also be widely used in robots, for example, it can be used for tracking and tracking of moving ends of surgical robots, industrial robots, etc. Correction.

The present disclosure also discloses the following:

1. A control method for a moving arm, comprising:

obtaining an image captured by an image capture device, the image including an image of a marker disposed on the end of the motion arm, the marker including a plurality of marker corners;

identifying marker corners in the acquired image; and

Based on the identified corner points of the marker, the current relative pose of the end of the moving arm relative to the image acquisition device is determined.

2. The control method according to item 1, further comprising:

determining a region of interest (ROI) in the acquired image;

identifying suspected marker corners in the ROI; and

Based on the standard corner model, a correlation test is performed on the corners of the suspected markers in the ROI to determine whether the corners of the suspected markers in the ROI image are marker corners.

3. The control method according to item 2, further comprising:

determining that the ROI is a full image, or determining that the ROI is a partial image based on the corner positions of the suspected markers in the previous frame of image; and

Convert the ROI to the corresponding grayscale image.

4. The control method according to item 3, wherein the partial image includes a set distance range centered on the imaginary point formed by the average coordinates of the corner points of the suspected marker in the previous frame of image, and the set distance includes multiple points. A predetermined multiple of the average separation distance between the corner points of the suspected markers.

5. The control method according to any one of items 2 to 4, wherein identifying a corner of a suspected marker in the ROI comprises:

Determine the corner likelihood value (CL) of each pixel in the ROI;

dividing the ROI into multiple sub-ROIs;

Determine the pixel with the largest CL value in each of the sub-ROIs;

Based on a plurality of pixel points with the largest CL value of the plurality of sub-ROIs, determining a set of pixel points whose CL value is greater than a first threshold;

determining the pixel point with the largest CL value in the pixel point set as the corner point of the first suspected marker; and

Based on the first suspected marker corner points, a second suspected marker corner point is searched along a plurality of edge directions with a set step size.

6. The control method according to item 5, further comprising:

determining the gradient direction and gradient weight of each pixel in the first predetermined range centered on the corner of the first suspected marker;

The plurality of edge directions are determined based on gradient directions and gradient weights of each pixel in the first predetermined range.

7. The control method according to any one of 5-6, further comprising:

Based on the first suspected marker corner point and the second suspected marker corner point, an edge direction is adjusted; and along the adjusted edge direction, a third suspected marker corner point is searched.

8. The control method according to any one of items 5-7, further comprising:

Based on the first suspected marker corner point and the second suspected marker corner point, the search is performed in the opposite edge direction.

9. The control method according to any one of items 5-7, further comprising:

in response to the number of suspected marker corners being greater than or equal to the second threshold, stopping the search; or

in response to the searched distance being greater than a predetermined multiple of the distance between the first suspected marker corner point and the second suspected marker corner point or in response to the determined number of suspected marker corner points being less than the second threshold, Based on the next pixel in the pixel point set in descending order of the CL value as the first suspected marker corner, search for the suspected marker corner along the edge direction with the set step size until the suspected marker corner The number of points is greater than or equal to the second threshold.

10. The control method according to any one of items 2 to 9, wherein the correlation test on the corner points of the suspected marker comprises:

determining a correlation coefficient between the grayscale distribution of the standard corner point model and the pixel grayscale distribution in a second predetermined range centered on the pixel point of the corner point of the suspected marker;

In response to the correlation coefficient being greater than a third threshold, the suspected marker corner is determined to be a marker corner.

11. The control method according to any one of items 2-9, further comprising:

In response to the suspected marker corner in the ROI being a marker corner, sub-pixel localization of the marker corner is performed.

12. The control method according to item 11, wherein the sub-pixel positioning of the marker corners comprises:

The coordinates of the marker corners are calculated based on the following formula:

S(x,y)=ax ² +by ² +cx+dy+exy+f

Among them, S(x, y) is the CL value fitting function of all pixels in each sub-ROI, and a, b, c, d, e, and f are coefficients;

Wherein, x _c is the x coordinate _{of the corner of the marker, and y c} is the y coordinate of the corner of the marker.

13. The control method according to any one of items 1-12, further comprising:

determining the three-dimensional coordinates of the marker corner points relative to the marker coordinate system;

determining the two-dimensional coordinates of the marker corners in the acquired image;

determining an analytical solution to the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates; and

Based on the analytical solution of the homography matrix, the current relative pose of the marker with respect to the image acquisition device is determined.

14. The control method according to item 13, wherein determining the three-dimensional coordinates of the marker corner points relative to the marker coordinate system comprises determining the three-dimensional coordinates of the marker corner points relative to the marker coordinate system based on the following formula: :

in,

is the three-dimensional coordinate corresponding to the corner point of the ith marker, r _wm is the radius of the cylinder formed by the marker, and β is the distribution angle of the corner point of the marker.

15. The control method of item 14, wherein determining the analytical solution of the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates comprises:

The analytical solution is calculated based on the following formula:

H _left =[h ₁ h ₂ h ₃ ]=η·A·[r ₁ r ₂ ^ll p _wm ] (2)

In formula (1),

Coordinates for the expansion of the i-th marker corner point, [u _i, v _i] is the i th corner marker two-dimensional coordinates, ^left s _i is an arbitrary nonzero scalar, ^left s _i is the scale factor, to Let the numbers in the last column on the left and right sides of equation (1) be the same, and H _left is a homography matrix; and

In formula (2), A is the known internal parameter matrix of the image acquisition device, r ₁ , r ₂ are the first two columns of the ^ll R _wm matrix, and η is an arbitrary non-zero scalar.

16. The control method according to item 15, wherein determining the analytical solution of the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates further comprises:

The analytical solution is calculated based on the following formula:

Wherein, x _H =[h ₁ ^T h ₂ ^T h ₃ ^T ] ^T , L is a 2n×9 matrix, n is the number of marker corner points determined in the acquired image, and x _H is the singular value decomposition of matrix L The right singular vector corresponding to the smallest singular value after (SVD).

17. The control method of item 16, further comprising:

The current relative pose of the marker relative to the image acquisition device is determined based on the following formula:

r ₁ =κ·A ⁻¹ ·h ₁

r ₂ =κ·A ⁻¹ ·h ₂

^ll R _wm =[r ₁ r ₂ r ₁ ×r ₂ ]

^ll p _wm =κ·A ^-1 h ₃

Here, κ=1/||A ⁻¹ ·h ₁ ||=1/||A ⁻¹ ·h ₂ ||.

18. The control method of any one of items 14-17, further comprising:

Based on the analytical solution of the homography matrix, a nonlinear optimization process is performed to obtain the current relative pose of the marker with respect to the image acquisition device.

19. The control method of item 18, performing the nonlinear optimization process comprising minimizing an average geometric error between the projection of the three-dimensional coordinates on the acquired image and the marker corners.

20. The control method according to item 19,

The image acquisition device is a single-lens image acquisition device, the acquired image is a single-lens image, and performing the nonlinear optimization process includes:

The optimization process is performed based on the following formula:

or

The image acquisition device is a dual-lens image acquisition device, the acquired images include a left-lens image and a right-lens image, and performing the nonlinear optimization process includes:

The optimization process is performed based on the following formula:

Among them, H _left and H _right are the homography matrices related to the ^left and ^right lens images, respectively; left s _i and right s _i are the analytical solutions of the homography matrices related to the left and right lens images, respectively the associated scale factor,

and

are the extended coordinates of the i-th marker corner point related to the left-lens image and the right-lens image, respectively.

21. The control method according to any one of items 1-20, further comprising:

Based on the current relative pose of the end of the moving arm, determine the current pose of the moving arm in the world coordinate system;

Based on the target pose and the current pose of the end of the moving arm in the world coordinate system, determining the difference between the target pose and the current pose of the end of the moving arm; and

A drive signal for the moving arm is determined based on the difference and an inverse kinematics numerical iterative algorithm of the moving arm.

22. The control method of item 21, further comprising:

At predetermined cycles, the drive signal of the motion arm is determined to implement a plurality of motion control cycles.

23. The control method of any one of items 1-22, further comprising:

determining a current relative pose of the marker relative to the image capture device based on the identified marker corner points; and

Based on the current relative pose of the marker and the following correspondence, the current pose of the end of the moving arm in the world coordinate system is determined:

^W p _tip = ^W R _ll ( ^ll R _wm ^wm p _tip + ^ll p _wm )+ ^W p _ll

Wherein, ^W p _tip is the position of the end of the moving arm in the world coordinate system {W}, and ^wmp _tip is the position of the end of the moving arm in the marker coordinate system {wm}; ^W R ₁₁ , ^W p ₁₁ are respectively the posture and position of the image acquisition device in the world coordinate system {W}; ¹¹ R _wm and ¹¹ p _wm are respectively the posture and position of the marker in the image acquisition device coordinate system {11};

in,

is the position coordinate of the end of the moving arm in the marker coordinate system {wm}.

24. The control method according to any one of items 1 to 23, wherein the marker has a cylindrical shape, comprising a plurality of marker corner points distributed on the cylindrical shape.

25. An exercise arm system comprising:

Image acquisition equipment for acquiring images;

at least one moving arm, including the end of the moving arm, the end of the moving arm is provided with a marker, the marker includes a plurality of marker corners;

A control device configured to perform the control method of any of items 1-24.

26. A computer-readable storage medium comprising one or more computer-executable instructions stored thereon, the computer-executable instructions being executed by a processor that has been configured to perform as described in any of items 1-24 control method.

Note that the above are merely exemplary embodiments of the present disclosure and applied technical principles. Those skilled in the art will understand that the present disclosure is not limited to the specific embodiments herein, and various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the protection scope of the present disclosure. Therefore, although the present disclosure has been described in detail through the above embodiments, the present disclosure is not limited to the above embodiments, and can also include more other equivalent embodiments without departing from the concept of the present disclosure. The scope is determined by the scope of the appended claims.

Claims (20)

1. A control method for a moving arm, comprising:

   obtaining an image captured by an image capture device, the image including an image of a marker disposed on the end of the motion arm, the marker including a plurality of marker corners;

   identifying marker corners in the acquired image; and

   Based on the identified corner points of the marker, the current relative pose of the end of the moving arm relative to the image acquisition device is determined.
2. The control method according to claim 1, further comprising:

   determining a region of interest (ROI) in the acquired image;

   identifying suspected marker corners in the ROI; and

   Based on the standard corner model, a correlation test is performed on the corners of the suspected markers in the ROI to determine whether the corners of the suspected markers in the ROI image are marker corners.
3. The control method according to claim 2, further comprising:

   determining that the ROI is a full image, or determining that the ROI is a partial image based on the corner positions of the suspected markers in the previous frame of image; and

   converting the ROI into a corresponding grayscale image;

   The partial image includes a set distance range centered on a virtual point formed by the average coordinates of the corner points of the suspected marker in the previous frame of image, and the set distance includes a plurality of average interval distances of the corner points of the suspected marker. predetermined multiples.
4. The control method according to claim 2, wherein identifying a corner of a suspected marker in the ROI comprises:

   Determine the corner likelihood value (CL) of each pixel in the ROI;

   dividing the ROI into multiple sub-ROIs;

   Determine the pixel with the largest CL value in each of the sub-ROIs;

   determining a set of pixel points with a CL value greater than a first threshold based on a plurality of pixel points with the largest CL value in the plurality of sub-ROIs;

   determining the pixel point with the largest CL value in the pixel point set as the corner point of the first suspected marker; and

   Based on the first suspected marker corner points, a second suspected marker corner point is searched along a plurality of edge directions with a set step size.
5. The control method according to claim 4, further comprising:

   determining the gradient direction and gradient weight of each pixel in the first predetermined range centered on the corner of the first suspected marker;

   The plurality of edge directions are determined based on gradient directions and gradient weights of each pixel in the first predetermined range.
6. The control method according to claim 4, further comprising:

   adjusting the edge orientation based on the first suspected marker corner point and the second suspected marker corner point; and

   along the adjusted edge direction, search for the corner point of the third suspected marker;

   Based on the first suspected marker corner point and the second suspected marker corner point, the search is performed in the opposite edge direction.
7. The control method according to claim 4, further comprising:

   in response to the number of suspected marker corners being greater than or equal to the second threshold, stopping the search; or

   in response to the searched distance being greater than a predetermined multiple of the distance between the first suspected marker corner point and the second suspected marker corner point or in response to the determined number of suspected marker corner points being less than the second threshold, Based on the next pixel in the pixel point set in descending order of the CL value as the first suspected marker corner, search for the suspected marker corner along the edge direction with the set step size until the suspected marker corner The number of points is greater than or equal to the second threshold.
8. The control method according to claim 2, wherein performing the correlation test on the corner points of the suspected marker comprises:

   determining a correlation coefficient between the grayscale distribution of the standard corner point model and the pixel grayscale distribution in a second predetermined range centered on the pixel point of the corner point of the suspected marker;

   In response to the correlation coefficient being greater than a third threshold, the suspected marker corner is determined to be a marker corner.
9. The control method according to claim 2, further comprising:

   In response to the suspected marker corner in the ROI being a marker corner, sub-pixel localization is performed on the marker corner;

   The sub-pixel positioning of the marker corners includes:

   The coordinates of the marker corners are calculated based on the following formula:

   S(x,y)=ax ² +by ² +cx+dy+exy+f

   Among them, S(x, y) is the CL value fitting function of all pixels in each sub-ROI, and a, b, c, d, e, and f are coefficients;

   

   Wherein, x _c is the x coordinate _{of the corner of the marker, and y c} is the y coordinate of the corner of the marker.
10. The control method according to claim 1, further comprising:

    determining the three-dimensional coordinates of the marker corner points relative to the marker coordinate system;

    determining the two-dimensional coordinates of the marker corners in the acquired image;

    determining an analytical solution to the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates; and

    Based on the analytical solution of the homography matrix, the current relative pose of the marker with respect to the image acquisition device is determined.
11. The control method according to claim 10, wherein determining the three-dimensional coordinates of the marker corner point relative to the marker coordinate system comprises determining the marker corner point relative to the marker coordinate system based on the following formula: 3D coordinates:

    

    in,

    

    is the three-dimensional coordinate corresponding to the corner point of the ith marker, r _wm is the radius of the cylinder formed by the marker, and β is the distribution angle of the corner point of the marker.
12. The control method according to claim 11, wherein determining the analytical solution of the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates comprises:

    The analytical solution is calculated based on the following formula:

    

    H _left =[h ₁ h ₂ h ₃ ]=η·A·[r ₁ r ₂ ^ll p _wm ] (2)

    In formula (1),

    

    is the extended coordinate of the corner point of the ith marker, [u _i ,v _i ] is the two-dimensional coordinates of the corner point of the ith marker, ^left s _i is an arbitrary non-zero scalar, ^left s _i is the scale factor, with Let the numbers in the last column on the left and right sides of equation (1) be the same, and H _left is a homography matrix; and

    In formula (2), A is the known internal parameter matrix of the image acquisition device, r ₁ , r ₂ are the first two columns of the ^ll R _wm matrix, and n is an arbitrary non-zero scalar;

    Determining the analytical solution of the homography matrix of the two-dimensional coordinates and the three-dimensional coordinates also includes:

    The analytical solution is calculated based on the following formula:

    

    Wherein, x _H =[h ₁ ^T h ₂ ^T h ₃ ^T ] ^T , L is a 2n×9 matrix, n is the number of marker corner points determined in the acquired image, and x _H is the singular value decomposition of matrix L The right singular vector corresponding to the smallest singular value after (SVD);

    It also includes determining the current relative pose of the marker relative to the image acquisition device based on the following formula:

    r ₁ =κ·A ⁻¹ ·h ₁

    r ₂ =κ·A ⁻¹ ·h ₂

    ^ll R _wm =[r ₁ r ₂ r ₁ ×r ₂ ]

    ^ll p _wm =κ·A ^-1 h ₃

    Here, κ=1/||A ⁻¹ ·h ₁ ||=1/||A ⁻¹ ·h ₂ ||.
13. The control method according to claim 10, further comprising:

    based on the analytical solution of the homography matrix, performing a nonlinear optimization process to obtain the current relative pose of the marker with respect to the image acquisition device;

    Performing the nonlinear optimization process includes minimizing an average geometric error between the projection of the three-dimensional coordinates on the acquired image and the marker corner points.
14. The control method according to claim 13, wherein,

    The image acquisition device is a single-lens image acquisition device, the acquired image is a single-lens image, and performing the nonlinear optimization process includes:

    The optimization process is performed based on the following formula:

    

    or

    The image acquisition device is a dual-lens image acquisition device, the acquired images include a left-lens image and a right-lens image, and performing the nonlinear optimization process includes:

    The optimization process is performed based on the following formula:

    

    Among them, H _left and H _right are the homography matrices related to the ^left and ^right lens images, respectively; left s _i and right s _i are the analytical solutions of the homography matrices related to the left and right lens images, respectively the associated scale factor,

    

    and

    

    are the extended coordinates of the corner point of the i-th marker related to the left-lens image and the right-lens image, respectively.
15. The control method according to claim 1, further comprising:

    Based on the current relative pose of the end of the moving arm, determine the current pose of the moving arm in the world coordinate system;

    Based on the target pose and the current pose of the end of the moving arm in the world coordinate system, determining the difference between the target pose and the current pose of the end of the moving arm; and

    A drive signal for the moving arm is determined based on the difference and an inverse kinematics numerical iterative algorithm of the moving arm.
16. The control method according to claim 15, further comprising:

    At predetermined cycles, the drive signal of the motion arm is determined to implement a plurality of motion control cycles.
17. The control method according to claim 1, further comprising:

    determining a current relative pose of the marker relative to the image capture device based on the identified marker corner points; and

    Based on the current relative pose of the marker and the following correspondence, the current pose of the end of the moving arm in the world coordinate system is determined:

    ^W p _tip = ^W R _ll ( ^ll R _wm ^wm p _tip + ^ll p _wm )+ ^W p _ll

    Wherein, _W p _tip is the position of the end of the moving arm in the world coordinate system {W}, ^wm p _tip is the position of the end of the moving arm in the marker coordinate system {wm}; ^W R ₁₁ , ^W p ₁₁ are the attitude and position of the image acquisition device in the world coordinate system {W} respectively; ¹¹ R _wm and ¹¹ p _wm are respectively the attitude and position of the marker in the image acquisition device coordinate system {11};

    

    in,

    

    is the position coordinate of the end of the moving arm in the marker coordinate system {wm}.
18. The control method according to claim 1, wherein the marker has a cylindrical shape and includes a plurality of marker corner points distributed on the cylindrical shape.
19. An exercise arm system comprising:

    Image acquisition equipment for acquiring images;

    at least one moving arm, including the end of the moving arm, the end of the moving arm is provided with a marker, the marker includes a plurality of marker corners;

    a control device configured to obtain an image captured by the image capture device, the image including an image of a marker disposed on the distal end of the moving arm; the control device further configured to identify the captured image and based on the identified marker corners, determine the current relative pose of the end of the moving arm relative to the image acquisition device.
20. A computer-readable storage medium comprising one or more computer-executable instructions stored thereon, the computer-executable instructions being executed by a processor having the processor configured to perform a control method, the method comprising:

    obtaining an image captured by the image capture device, the image including an image of a marker disposed on the end of the moving arm, the marker including a plurality of marker corners;

    identifying marker corners in the acquired image; and

    Based on the identified corner points of the marker, the current relative pose of the end of the moving arm relative to the image acquisition device is determined.

Images