CN114536292A - Error detection method based on composite identification and robot system - Google Patents

Abstract

The disclosure relates to the technical field of error detection, and discloses an error detection method. The error detection method comprises the following steps: obtaining a target pose of the tail end of the execution arm; acquiring a positioning image; identifying a plurality of markers located on the end of the actuator arm in the positioning image, the plurality of markers including a plurality of pose markers for identifying a pose and at least one composite marker for identifying the pose and an angle; determining an actual pose of the tip of the effector arm based on the at least one composite signature and the plurality of pose signatures; and generating a fault-related control signal in response to the target pose and the actual pose satisfying an error detection condition.

Description

Error detection method based on composite identification and robot system

Technical Field

The present disclosure relates to the field of error detection technologies, and in particular, to an error detection method and a robot system based on a composite identifier.

Background

Generally, a robot system for teleoperation includes an execution arm for performing an operation and a main manipulator for controlling movement of the execution arm. In a practical scenario, the actuator arm is configured to enter an operation area, and an operator controls the movement of the actuator arm in the operation area by teleoperating the main operator, and then operates the actuator arm through an end (actuator) provided at the actuator arm. The robot realizes the motion control of the main operator to the execution arm through the motion conversion between the main operator and the execution arm.

The robot has high requirements on operation precision and human-computer interaction experience. In the teleoperation process, the pose error of the execution arm needs to be detected in real time to determine whether the execution arm correctly moves to the position and the posture corresponding to the operation of the main operator according to the intention of the operator, and further the working condition of the robot system is grasped in real time.

Disclosure of Invention

In some embodiments, the present disclosure provides an error detection method. The method can comprise the following steps: obtaining a target pose of the tail end of the execution arm; acquiring a positioning image; identifying a plurality of markers located on the end of the actuator arm in the positioning image, the plurality of markers including a plurality of pose markers for identifying a pose and at least one composite marker for identifying the pose and an angle; determining an actual pose of the tip of the effector arm based on the at least one composite signature and the plurality of pose signatures; and generating a fault-related control signal in response to the target pose and the actual pose satisfying an error detection condition.

In some embodiments, the present disclosure provides a computer device comprising: a memory for storing at least one instruction; and a processor coupled with the memory and configured to execute at least one instruction to perform the method of any of some embodiments of the present disclosure.

In some embodiments, the present disclosure provides a computer-readable storage medium storing at least one instruction that, when executed by a computer, causes the computer to perform the method of any of some embodiments of the present disclosure.

In some embodiments, the present disclosure provides a robotic system comprising: the main manipulator comprises a mechanical arm, a handle arranged on the mechanical arm and at least one main manipulator sensor arranged at least one joint on the mechanical arm, wherein the at least one main manipulator sensor is used for acquiring joint information of the at least one joint; the tail end of the execution arm is provided with a plurality of marks, and the marks comprise a plurality of pose marks and at least one composite mark; at least one drive device for driving the actuating arm; at least one drive sensor coupled to the at least one drive and configured to obtain status information of the at least one drive; the image acquisition equipment is used for acquiring a positioning image of the execution arm; and a control device configured to be connected with the main operator, the at least one driving device, and the at least one driving device sensor image acquisition equipment, and to execute the method of any one of some embodiments of the present disclosure.

Drawings

FIG. 1 illustrates a schematic structural diagram of a robotic system according to some embodiments of the present disclosure;

FIG. 2 illustrates a schematic diagram of an error detection system according to some embodiments of the present disclosure;

FIG. 3 illustrates a flow diagram of an error detection method according to some embodiments of the present disclosure;

FIG. 4 illustrates a flow chart of a method of determining a target pose of an end of an execute arm according to some embodiments of the present disclosure;

FIG. 5 illustrates a coordinate system diagram in a master-slave motion map, according to some embodiments of the present disclosure;

FIG. 6 illustrates a schematic diagram of a tag including multiple identifiers, according to some embodiments of the present disclosure;

FIG. 7 shows a schematic view of a label disposed around the distal end of an actuator arm and formed into a cylindrical shape, according to some embodiments of the present disclosure;

FIG. 8 illustrates a schematic diagram of an implementation scenario, according to some embodiments of the present disclosure;

FIG. 9 illustrates a flow chart of a method of determining an actual pose of an end of an implement arm according to some embodiments of the present disclosure;

FIG. 10 illustrates a flow chart of a method of determining an actual pose of an end of an effector arm according to further embodiments of the present disclosure;

FIG. 11 illustrates a flow diagram of a method for identifying an identity, according to some embodiments of the present disclosure;

fig. 12 shows a schematic view of a pose identification pattern according to some embodiments of the present disclosure;

FIG. 13 illustrates a flow diagram of a method for searching for an identity, according to some embodiments of the present disclosure;

FIG. 14 illustrates a schematic diagram of search identification, according to some embodiments of the present disclosure;

FIG. 15 shows a schematic block diagram of a computer device in accordance with some embodiments of the present disclosure;

fig. 16 shows a schematic view of a robotic system according to some embodiments of the present disclosure.

Detailed Description

In order to make the technical problems solved, technical solutions adopted, and technical effects achieved by the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. Those skilled in the art will appreciate that the described embodiments are to be considered in all respects only as illustrative and not restrictive, and that the present disclosure provides exemplary embodiments, rather than 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", and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, and are only for convenience of describing and simplifying the present disclosure, but do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and thus, should not be construed as limiting the present disclosure. Furthermore, the terms "first" and "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.

In the description of the present disclosure, it is to be noted that, unless otherwise explicitly specified or limited, the terms "mounted," "connected," and "coupled" are to be construed broadly and can include, for example, fixed and removable connections; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium; there may be communication between the interiors of the two elements. The specific meanings of the above terms in the present disclosure can be understood by those of ordinary skill in the art as appropriate. In the present disclosure, the end close to the operator is defined as a proximal end, a proximal portion, or a rear end, a rear portion, and the end close to the work object is defined as a distal end, a distal portion, or a front end, a front portion. It will be appreciated by those skilled in the art that embodiments of the present disclosure may be applied to implement arms disposed on machinery operating in a variety of environments including, but not limited to, above ground, below ground, underwater, space, and within organisms.

In the present disclosure, the term "position" refers to the positioning of an object or a portion of an object in three-dimensional space (e.g., three translational degrees of freedom may be described using cartesian X, Y and changes in Z coordinates, such as along cartesian X, Y, and Z axes, respectively). In this disclosure, the term "pose" refers to a rotational setting of an object or a portion of an object (e.g., three rotational degrees of freedom that can 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 portion of an object, such as may be described using six parameters in the six degrees of freedom mentioned above. In the present disclosure, the pose of the handle of the master manipulator may be represented by a set of joint information of the master manipulator joints (e.g., a one-dimensional matrix composed of these joint information). The pose of the actuator arm may be determined by the drive information of the actuator arm. In the present disclosure, the joint information of the joints may include an angle by which the corresponding joint rotates with respect to the corresponding joint axis or a distance moved from an initial position.

In the present disclosure, a reference coordinate system may be understood as a coordinate system capable of describing the pose of an object. According to the actual positioning requirement, the reference coordinate system can be selected to use the origin of the virtual reference object or the origin of the physical reference object as the origin of the coordinate system. In some embodiments, the reference coordinate system may be a world coordinate system or a main operator, an execution arm, a coordinate system of a space in which the camera is located, or a perception coordinate system of the operator himself, or the like.

In the present disclosure, an object may be understood as an object or target that needs to be positioned, such as an actuator arm or the tip of an actuator arm. The pose of the effector arm or a portion thereof (e.g., the tip) may refer to the pose of a coordinate system defined by the effector arm or a portion thereof relative to a reference coordinate system.

Fig. 1 illustrates a schematic structural diagram of a robotic system 100 according to some embodiments of the present disclosure. In some embodiments, as shown in fig. 1, the robotic system 100 may include a master trolley 110, a slave trolley 130, and a control device 120. The control device 120 may be communicatively coupled to the master trolley 110 and the slave trolley 130, for example, by a cable or wirelessly, to enable communication between the master trolley 110 and the slave trolley 130. The master trolley 110 includes a master operator for operator teleoperation, and the slave trolley 130 includes at least one execution arm for performing a job. The master-slave mapping between the master manipulator in the master trolley 110 and the execution arm in the slave trolley 130 is realized by the control device 120, so that the motion control of the execution arm by the master manipulator is realized. In some embodiments, the implement arm is configured to be able to access an operating area through a sheath, or the like, where the sheath, or the like may be secured to a wall, animal body, or the like where an opening (e.g., an artificial or natural opening) is formed, and the operating area may be the area where work is performed. The implement arm may be a continuous body deformable arm, and an end instrument (e.g., an effector) may be disposed at a distal end of the implement arm, which may include, but is not limited to, a digging instrument, an underwater working instrument, a sorting instrument, a surgical instrument, and the like. Those skilled in the art will appreciate that the master trolley 110 and the slave trolley 130 may take other configurations or forms, such as a base, a stand, a building, or the like.

In some embodiments, the implement arm may be used as a vision tool in addition to a work tool, and the end instruments of the vision tool may include, but are not limited to, an image capture device or an illumination device, etc. In some embodiments, the master trolley may include a master operator and a display for displaying an image of the operating area. The image acquisition device may be configured to acquire an image of the operating area and transmit the acquired image to the slave trolley. And the image is processed by a video processing module in the driven trolley and then displayed on a display of the driven trolley. The pose of the end of the actuator arm relative to the reference coordinate system is obtained in real time by the operator through the image in the display. The pose of the main operator with respect to the reference coordinate system is the pose that the operator really perceives. The pose change sensed by the operator through teleoperation of the main operator and the pose change of the tail end of the execution arm sensed by the operator in the display accord with a preset pose relation, so that the pose of the main operator is transformed into the pose change of the tail end of the execution arm based on the preset pose relation through teleoperation of the main operator, and the pose control of the tail end of the execution arm is further realized. In this way, when the operator grips the handle of the main operator to operate the execution arm, the posture variation of the distal end instrument of the execution arm felt by the operator is consistent with the posture variation of the main operator felt by the operator based on the principle of intuitive operation, which contributes to the improvement of the teleoperation feeling and teleoperation accuracy of the operator.

During the teleoperation, the actuator arm sometimes cannot be moved accurately to a position and posture corresponding to the operation of the main operator as desired by the operator. In the method, the pose error of the executing arm is detected in the teleoperation process, whether the executing arm moves correctly according to the intention of an operator is determined, and the working condition of the robot system is further grasped in real time. Those skilled in the art will appreciate that the pose error detection methods according to some embodiments of the present disclosure may also be performed during non-teleoperation.

Fig. 2 illustrates a schematic diagram of an error detection system 200 according to some embodiments of the present disclosure. As shown in fig. 2, the system 200 may include a main operator 210, a control device 220, an execution arm 230, and an image acquisition apparatus 250. The actuator arm 230 may be implemented as a deformable arm or a rigid arm. In some embodiments, the effector arm 230 may include an effector arm tip 231 at the distal end or distal end, and a tip instrument 240 may be disposed on the effector arm tip 231.

The control device 220 may be communicatively coupled to at least one drive device to send drive information to the drive device to control the movement of the actuator arm 230 to move the actuator arm tip 231 to a desired position and attitude. For example, the at least one driving device for controlling the movement of the actuator arm 230 may be a servo motor, and may receive the command from the control device to control the movement of the actuator arm 230. In some embodiments, the control device 220 may determine the target pose of the effector arm tip 231 based on the pose of the primary manipulator 210 and the mapping between the primary manipulator 210 and the effector arm 230.

The image capturing device 250 is communicatively connected to the control means 220. In some embodiments, image capture device 250 may be used to capture positioning images, and image capture device 250 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. The positioning image may include an image of some or all of the effector arm 230 located within the operating field. In some embodiments, the image capture device 250 may be used to capture images of the actuator arm tip 231. The actuator arm end 231 may be provided with a plurality of markers including marker patterns and pattern corner points. For example, the actuator arm end 231 may be provided with a positioning tab 232 (the positioning tab 232 may be, for example, the tab 600 shown in fig. 6). The position tags 232 may include a plurality of markers including a plurality of pose markers for identifying a pose and at least one composite marker for identifying a pose and an angle (described in detail below).

As shown in fig. 2, the actuator arm end 231 is within the field of view 251 of the image capturing device 250, and the captured positioning image may include an image of the actuator arm end 231. The image capture device 250 may be an industrial camera, an underwater camera, a miniature electronic camera, an endoscopic camera, etc., depending on the application scenario. In some embodiments, the image capture device 250 may be fixed in position or variable in position, for example, an industrial camera fixed in a monitoring position or an endoscopic camera with adjustable position or pose. In some embodiments, the image capturing device 250 may implement at least one of visible light band imaging, infrared band imaging, CT (Computed Tomography) imaging, acoustic wave imaging, and the like. Depending on the kind of the captured image, a person skilled in the art may select different image capturing devices as the image capturing device 250.

In some embodiments, the control means 220 may receive positioning images from the image acquisition device 250 and process the positioning images. For example, the control device 220 may identify a plurality of markers located on the actuator arm tip 231 in the positioning image and determine the pose of the actuator arm tip 231 relative to a reference coordinate system (e.g., a world coordinate system) as the actual pose of the actuator arm tip 231.

In the present disclosure, the control device 220 may perform error detection on the actuator arm 230 based on the target pose and the actual pose of the actuator arm end 231, determine whether the actuator arm end 231 has accurately moved to the position and the pose corresponding to the operation of the main operator 210, and then determine whether the actuator arm 230 has failed and generate a corresponding control signal. In some embodiments, the control device 220 may also determine the target pose and the actual pose of the actuator arm tip 231 at predetermined periods to perform error detection on the actuator arm 230 in real time through multiple detection cycles. Those skilled in the art will appreciate that the system 200 may be applied to special purpose or general purpose robotic systems in a number of fields (e.g., medical, industrial manufacturing, etc.), such as the robotic system 100 shown in fig. 1, or the robotic system 1600 shown in fig. 16. As an example, the system 200 may be applied to a robotic system, such as a surgical robot, and the end instrument 240 provided at the distal end of the effector arm end 231 may be, for example, a surgical effector.

Some embodiments of the present disclosure provide an error detection method for an actuator arm of a robotic system. Fig. 3 illustrates a flow diagram of an error detection method 300 (hereinafter also simply "method 300"), according to some embodiments of the present disclosure. The method 300 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 300 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1600 shown in fig. 16). In some embodiments, the method 300 may be implemented as computer readable instructions. The instructions may be read and executed by a general-purpose processor or a special-purpose processor (e.g., control device 120 shown in fig. 1, control device 220 shown in fig. 2, or control device 1620 shown in fig. 16). For example, a control apparatus for a robotic system may include a processor configured to perform method 300. In some embodiments, these instructions may be stored on a computer-readable medium.

Referring to fig. 3, in step 301, a target pose of the tip of the actuator arm is obtained. In some embodiments, the target pose of the tip end of the execution arm may be determined from the pose of the primary operator based on a master-slave motion mapping relationship between the pose of the primary operator and the pose of the tip end of the execution arm. An exemplary method of obtaining the target pose of the tip of the effector arm includes a method as shown in fig. 4.

Fig. 4 illustrates a flow diagram of a method 400 of determining a target pose of an end of an effector arm according to some embodiments of the present disclosure. As shown in fig. 4, some or all of the steps of the method 400 may be performed by a control device (e.g., the control device 120 shown in fig. 1, the control device 220 shown in fig. 2, or the control device 1620 shown in fig. 16). Some or all of the steps in method 400 may be implemented by software, firmware, and/or hardware. In some embodiments, method 400 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1 or robotic system 1600 shown in fig. 16). In some embodiments, the method 400 may be implemented as computer-readable instructions. These instructions may be read and executed by a general-purpose processor or a special-purpose processor, such as control 1620 shown in fig. 16. In some embodiments, these instructions may be stored on a computer-readable medium.

Fig. 5 illustrates a coordinate system diagram in a master-slave motion map, according to some embodiments of the present disclosure. The definition of each coordinate system in fig. 5 is as follows: the base coordinate of the execution arm is Tb, the origin is at the base of the execution arm or at the exit of the entry sheath,

in line with the axial direction of the base extension or into the sheath,

the direction is as shown in figure 5Shown in the figure. Camera coordinate system { lens }, origin is located at camera center, and camera axis direction is

Direction, right back and upper view

And (4) direction. The coordinate system of the end of the execution arm { wm }, the origin is located at the end of the execution arm,

in line with the axial direction of the tip,

the orientation is shown in figure 5. The reference coordinate system w may be the coordinate system of the space in which the master operator or the execution arm or camera is located, e.g. the execution arm base coordinate system Tb, or the world coordinate system, as shown in fig. 5. In some embodiments, the body feeling of the operator can be used as a reference, and when the operator sits up in front of the main control console, the body feeling is upward

Direction, somatosensory forward direction is

And (4) direction. Display coordinate system { Screen }, origin at center of display, and inward direction perpendicular to Screen picture

In the positive direction, the upper part of the screen picture is

And (4) direction. The main operator has a coordinate system { CombX }, and the directions of coordinate axes are shown in FIG. 5. The handle coordinate system { H } of the main operator, coordinate axis directions are shown in FIG. 5.

A method 400 of determining the pose of an object at the tip of an actuator arm is described below using the coordinate system shown in fig. 5 as an example. However, those skilled in the art will appreciate that other coordinate system definitions may be employed to implement the method 400 of determining the pose of an object at the tip of an effector arm.

Referring to FIG. 4, in step 401, a current pose of the primary operator may be determined, the current pose including a current position and a current pose. In some embodiments, the current pose of the primary operator is the pose relative to the primary operator base coordinate system { CombX }. For example, the pose of the master operator is the pose of the handle of the master operator, or a portion thereof, defined coordinate system relative to the master operator base coordinate system { CombX } (e.g., the coordinate system defined by the stand or base on which the master operator is located, or the world coordinate system). In some embodiments, determining the current position of the master operator comprises determining a current position of the grip of the master operator relative to a master operator base coordinate system { CombX }, and determining the current pose of the master operator comprises determining a current pose of the grip of the master operator relative to the master operator base coordinate system { CombX }.

In some embodiments, the current pose of the primary operator may be determined based on the coordinate transformation. For example, the current pose of the handle may be determined based on a transformation relationship between the coordinate system { H } of the handle of the master operator and the master operator base coordinate system { CombX }. Typically, the main operator base coordinate system { CombX } may be located on a stand or base on which the main operator is located, and the main operator base coordinate system { CombX } remains unchanged during the teleoperation.

In some embodiments, a current pose of the primary operator may be determined based on the primary operator sensor. In some embodiments, current joint information for at least one joint of the primary manipulator is received, and a current pose of the primary manipulator is determined based on the current joint information for the at least one joint. For example, the current pose of the master operator is determined based on current joint information of the at least one joint obtained by the master operator sensor. The main operator sensor is provided at least one joint position of the main operator. For example, the master operator includes at least one joint at which at least one master operator sensor is disposed. The current pose of the master operator is calculated based on the joint information (position or angle) of the corresponding joint acquired by the master operator sensor. For example, the current position and the current attitude of the main operator are calculated based on a forward kinematics algorithm.

In some embodiments, the master manipulator includes at least one pose joint for controlling the pose of the handle. Determining the current pose of the handle of the master operator includes: joint information of at least one attitude joint is obtained, and based on the joint information of the at least one attitude joint, a current attitude of the main operator is determined. The main manipulator comprises a mechanical arm, and the mechanical arm comprises a position joint and a posture joint. The attitude joint adjusts the attitude of the main manipulator, and the main manipulator is controlled to reach the target attitude through one or more attitude joints. The position joints adjust the position of the main operator, and one or more position joints control the main operator to reach the target position. The main operator sensor is provided at the attitude joint and the position joint of the robot arm, and acquires joint information (position or angle) corresponding to the attitude joint and the position joint. According to the acquired joint information, the current pose of the handle of the main operator relative to the basic coordinate system { CombX } of the main operator can be determined. For example, the main operator may include 7 joints, wherein the joint 5, the joint 6, and the joint 7 are attitude joints for adjusting the attitude of the handle of the main operator. The current attitude of the main operator is calculated based on joint information (such as an angle) acquired by a main operator sensor of the attitude joint and a forward kinematics algorithm. The joints 1, 2, and 3 are position joints for adjusting the position of the handle of the main operator. The current position of the master operator is calculated based on joint information (e.g., position) acquired by the master operator sensor of the positional joint and a forward kinematics algorithm.

In step 403, a target pose of the tip of the effector arm may be determined based on the current pose of the primary operator and the pose relationship of the primary operator to the tip of the effector arm. For example, a master-slave mapping relationship between the master manipulator and the end of the execution arm is established, and the pose of the end of the execution arm is controlled by teleoperation of the master manipulator. The pose relationship includes a relationship between the pose of the tip of the actuator arm relative to the reference coordinate system { w } and the pose of the master operator relative to the reference coordinate system { w }. The reference coordinate system w includes the coordinate system of the space in which the main operator or the execution arm or the camera is located or the world coordinate system.

In some embodiments, the pose relationship between the master operator and the tip of the implement arm may include a relationship, such as equal or proportional, between an amount of pose change of the master operator and an amount of pose change of the tip of the implement arm. Determining the target pose of the tip of the effector arm comprises: the method includes determining a previous pose of the primary operator, determining a starting pose of the tip of the effector arm, and determining a target pose of the tip of the effector arm based on the previous and current poses of the primary operator and the starting pose of the tip of the effector arm. The previous pose and the current pose of the master operator may be the pose of the master operator's handle relative to the master operator base coordinate system { CombX }. The starting pose and target pose of the end of the actuator arm may be the poses of the end of the actuator arm relative to the actuator arm base coordinate system { Tb }.

The pose of the tip of the actuator arm may comprise the pose of the tip coordinate system { wm } of the actuator arm relative to the actuator arm base coordinate system { Tb }. The base coordinate system of the actuation arm { Tb } may be the coordinate system of the base on which the actuation arm is mounted, the coordinate system of the sheath through which the tip of the actuation arm passes (e.g., the coordinate system of the sheath exit), the coordinate system of the proximal Center of Motion (RCM) of the actuation arm, and so forth. For example, the base coordinate system of the execution arm { Tb } may be located at the sheath exit position, and the base coordinate system of the execution arm { Tb } remains unchanged during teleoperation. The starting pose of the tip of the actuator arm may be transformed to a coordinate system, resulting in a pose relative to another coordinate system (e.g., a reference coordinate system).

In some embodiments, previous joint information for at least one joint of the master manipulator may be received, and a previous pose of the master manipulator may be determined based on the previous joint information for the at least one joint. For example, a previous pose and a current pose of a handle of the master operator are determined based on the master operator sensor reading joint information of the master operator at a previous time and a current time. The amount of change in the position of the handle of the main operator is determined based on the previous position of the handle relative to the base coordinate system { CombX } of the main operator and the current position. Determining the posture variation of the handle of the main operator based on the previous posture and the current posture of the handle relative to the basic coordinate system { CombX } of the main operator.

In some embodiments, the actual pose of the end of the actuator arm obtained in the previous detection cycle may be received as the starting pose of the end of the actuator arm in the current detection cycle. For example, in each detection cycle, the camera may capture a positioning image of the end of the actuator arm, from which a plurality of markers located on the end of the actuator arm may be identified, and thereby determine an actual pose of the end of the actuator arm (described in detail below), which may be used as a starting pose of the end of the actuator arm in a next detection cycle. For example, for a first round of detection cycles, then an initial pose of the tip of the actuator arm (e.g., zero position of the actuator arm) may be employed as the starting pose for the first round of detection cycles.

In some embodiments, the pose change amount of the primary operator may be determined based on a previous pose of the primary operator and a current pose. The pose variation amount of the tip of the actuator arm may be determined based on the pose variation amount of the main operator and the pose relationship between the main operator and the tip of the actuator arm. The target pose of the tip end of the effector arm may be determined based on the starting pose of the tip end of the effector arm and the pose variation amount of the tip end of the effector arm.

The pose relationship may include a position relationship and a pose relationship. The positional relationship between the main operator and the tip of the actuator arm may include a relationship, such as equal or proportional, between an amount of positional change of the main operator and an amount of positional change of the tip of the actuator arm. The attitude relationship between the master manipulator and the tip of the implement arm may include a relationship, such as equal or proportional, between an amount of change in the attitude of the master manipulator and an amount of change in the attitude of the tip of the implement arm.

In some embodiments, the method 400 further comprises: the method includes determining a current position of a handle of a main operator relative to a main operator base coordinate system, determining a previous position of the handle relative to the main operator base coordinate system, determining a starting position of a tip of an actuator arm relative to an actuator arm base coordinate system, and determining a target position of the tip of the actuator arm relative to the actuator arm base coordinate system based on the previous position and the current position of the handle relative to the main operator base coordinate system, a transformation relationship of the actuator arm base coordinate system and the main operator base coordinate system, and the starting position of the tip of the actuator arm relative to the actuator arm base coordinate system. For example, the previous position of the main operator is determined based on the joint information corresponding to the main operator at the previous time read by the main operator sensor, and the current position of the main operator is determined based on the joint information corresponding to the main operator at the current time read by the main operator sensor. The amount of change in the position of the main operator is determined based on the previous position of the handle relative to the base coordinate system { CombX } of the main operator and the current position. And determining the starting position of the tail end of the executing arm based on the actual pose of the tail end of the executing arm obtained in the last round of detection. And determining the position variation of the tail end of the execution arm based on the position variation of the main operator and the pose relation of the main operator and the tail end of the execution arm. The target position of the tip of the actuator arm is determined based on the start position of the tip of the actuator arm and the amount of change in the position of the tip of the actuator arm.

In some embodiments, the method 400 further comprises: the method includes determining a current pose of a handle of a main operator relative to a main operator base coordinate system, determining a previous pose of the handle relative to the main operator base coordinate system, determining a starting pose of a tip of an implement arm relative to an implement arm base coordinate system, and determining a target pose of the tip of the implement arm relative to the implement arm base coordinate system based on the previous and current poses of the handle relative to the main operator base coordinate system, a transformation relationship of the implement arm base coordinate system to the main operator base coordinate system, and the starting pose of the tip of the implement arm relative to the implement arm base coordinate system. For example, the previous attitude of the master operator is determined based on the joint information of the master operator read by the master operator sensor corresponding to the previous time, and the current attitude of the master operator is determined based on the joint information of the master operator read by the master operator sensor corresponding to the current time. The pose change amount of the main operator is determined based on the previous pose of the handle with respect to the base coordinate system { CombX } of the main operator and the current pose. And determining a starting posture of the tail end of the executing arm based on the actual pose of the tail end of the executing arm obtained in the last round of detection. And determining the attitude change quantity of the tail end of the execution arm based on the attitude change quantity of the main operator and the attitude relationship between the main operator and the tail end of the execution arm. A target pose of the tip of the effector arm is determined based on the starting pose of the tip of the effector arm and the pose variation of the tip of the effector arm.

In some embodiments, the pose relationships include: the amount of change in the position of the tip of the actuator arm in the reference coordinate system { w } is proportional to the amount of change in the position of the main operator in the reference coordinate system { w }, and can be expressed as:

^wΔP_wm＝k·^wΔP_H (1)

in the formula (1), the left side^wΔP_wmIndicating the amount of change in the position of the end of the actuator arm relative to the reference coordinate system w, right side^wΔP_HIndicating the amount of change in the position of the master operator relative to the reference coordinate system w. And the number of the first and second electrodes,^wΔP_wmand^wΔP_Hin a proportional relationship, the proportionality coefficient is k.

In some embodiments, the method may be based on a previous position of the host operator relative to the reference coordinate system { w }^wP_H(t0)And current position^wP_HDetermining a position change amount of a master operator^wΔP_H. For example, at time t0 during the teleoperation, the previous position of the handle of the main operator with respect to the reference coordinate system { w } may be determined based on the joint information of the main operator acquired by the main operator sensor^wP_H(t0). At time t1 during the remote operation, the current position of the grip of the main operator with respect to the reference coordinate system { w } may be determined based on the joint information of the main operator acquired by the main operator sensor^wP_H. Based on the previous position of the master operator at time t0^wP_H(t0)And the current position of the main operator at time t1^wP_HObtaining the position variation of the main operator^wΔP_H. In some embodiments, the time t0 to the time t1 may include a plurality of control cycles of the execution arm, the time t0 may be a time at which the teleoperation instruction is triggered or a time at which the plurality of control cycles start, and the time t1 may be a time at which the teleoperation instruction ends or a time at which the plurality of control cycles complete.

In some embodimentsMay be based on the starting position of the end of the actuator arm relative to the reference coordinate system w^wP_wmSAnd target position^wP_wmTDetermining the amount of change in the position of the end of the actuator arm^wΔP_wm. In some embodiments, a detection loop of the execution arm (e.g., t 0-t 1) may encompass multiple control loops of the execution arm. For example, the previous detection cycle of the execution arm may end at time t0, and the current detection cycle of the execution arm may start at time t0 and end at time t 1. In some embodiments, the actual position in the actual pose of the tip of the actuator arm obtained in the previous detection cycle (e.g., at time t 0) may be compared^wP_wmR(t0)Determined as the starting position of the end of the execution arm in the current detection cycle with respect to the reference coordinate system { w }^wP_wmS. Can be changed based on the position of the handle^wΔP_HAnd the starting position of the end of the actuator arm relative to the reference coordinate system w^wP_wmSDetermining a target position of the end of the actuator arm relative to a reference coordinate system { w }^wP_wmT。

In the formula (1), the amount of change in the position of the tip of the arm with respect to the reference coordinate system { w }^wΔP_wmTarget position of the end of the actuator arm relative to the reference coordinate system w^wP_wmTAnd the starting position of the end of the actuator arm (e.g., at time t 0) relative to the reference frame { w }^wP_wmSIs expressed as a difference value of (a), as shown in equation (2),

^wΔP_wm＝^wP_wmT-^wP_wmS (2)

in the formula (1), the amount of change in the position of the main operator with respect to the reference coordinate system { w }^wΔP_HThe current position of the main operator (e.g., at time t1) relative to the reference coordinate system w^wP_HAnd the previous position of the master operator (e.g., at time t 0) relative to the reference coordinate system { w }^wP_H(t0)Is expressed as a difference value of (a), as shown in equation (3),

^wΔP_H＝^wP_H-^wP_H(t0) (3)

in some embodiments, the same matrix is multiplied by the left and right sides of equation (1), respectively^TbR_wObtaining a formula (4) based on the formulas (1) to (3),

^TbR_w(^wP_wmT-^wP_wmS)＝k·^TbR_w(^wP_H-^wP_H(t0)) (4)

equation (5) is derived based on the left side of equation (4),

^TbR_w(^wP_wmT-^wP_wmS)＝^TbP_wmT-^TbP_wmS (5)

equation (6) is derived based on the right hand side of equation (4),

k·^TbR_w(^wP_H-^wP_H(t0))＝k·^TbR_CombX(^CombXP_H-^CombXP_H(t0)) (6)

obtaining a formula (7) based on the formula (5) and the formula (6),

^TbP_wmT＝k·^TbR_CombX(^CombXP_H-^CombXP_H(t0))+^TbP_wmS (7)

based on equation (7), in some embodiments, the previous position of the handle relative to the host operator base coordinate system { CombX } may be based^CombXP_H(t0)And current position^CombXP_HThe current position of the end of the execution arm relative to the execution arm base coordinate system { Tb }^TbP_wmSThe transformation relationship between the base coordinate system { CombX } of the main operator and the base coordinate system { Tb } of the execution arm^TbR_CombXDetermining a target position of the end of the actuator arm relative to the base coordinate system of the actuator arm { Tb }^TbP_wmT。

In some embodiments, the pose of the tip of the effector arm in the reference coordinate system { w } coincides with the pose of the master manipulator in the reference coordinate system { w }. In some embodiments, the attitude change amount of the tip of the execution arm with respect to the reference coordinate system { w } is consistent with the attitude change amount of the main operator with respect to the reference coordinate system { w }, and may be expressed as:

^wR_wmS-wmT＝^wR_H(t0)-H (8)

in the formula (8), the left side^wR_wmS-wmTIndicating the amount of change in attitude of the tip of the actuator arm relative to the reference coordinate system { w }, right side^wR_H(t0)-HRepresenting the amount of change in the attitude of the host operator relative to the reference coordinate system w.

In some embodiments, the previous pose of the master operator with respect to the reference coordinate system { w } may be based on^wR_H(t0)And current attitude^wR_HDetermining a change in pose of a master operator^wR_H(t0)-H. For example, at time t0 in the teleoperation, the previous posture of the grip of the main operator with respect to the reference coordinate system { w } may be determined based on the joint information of the main operator acquired by the main operator sensor^wR_H(t0₎. At time t1 during the remote operation, the current posture of the grip of the main operator with respect to the reference coordinate system { w } may be determined based on the joint information of the main operator acquired by the main operator sensor^wR_H. May be based on the previous pose of the master operator at time t0^wR_H(t0)And the current attitude of the master operator at time t1^wR_HObtaining the attitude variation of the main operator^wR_H(t0)-H. Similarly, in some embodiments, time t0 to time t1 may correspond to a single detection cycle, which may include multiple control cycles of the execution arm, time t0 may be the time at which the teleoperation instruction is triggered or the time at which the detection cycle begins, and time t1 may be the time at which the teleoperation instruction ends or the time at which the detection cycle completes.

In some embodiments, the starting pose of the tip of the effector arm with respect to the reference coordinate system { w } may be based on^wR_wmSAnd target posture^wR_wmTDetermining the amount of change in attitude of the end of the actuator arm^wR_wmS-wmT. Likewise, in some embodiments, a detection loop of the arm is performed (e.g., t 0-t)t1) may cover multiple control cycles of the actuator arm. For example, the previous detection cycle of the execution arm may end at time t0, and the current detection cycle of the execution arm may start at time t0 and end at time t 1. In some embodiments, the actual pose in the actual pose of the tip of the implement arm obtained from the previous round of the detection cycle (e.g., at time t 0) may be compared^wR_wmR(t0)Determined as the starting pose of the end of the execution arm in the current detection cycle with respect to the reference coordinate system { w }^wR_wmS. Can change the quantity based on the posture of the handle^wR_H(t0)-HAnd a starting pose of the end of the actuator arm with respect to the reference coordinate system w^wR_wmSDetermining a target pose of the end of the actuator arm relative to a reference coordinate system { w }^wR_wmT。

In the formula (8), the posture variation amount of the posture of the tip end of the arm with respect to the reference coordinate system { w }is performed^wR_wmS-wmTMay be based on a starting pose of the end of the execution arm with respect to a reference coordinate system w^wP_wmSAnd target pose of the tip of the actuator arm with respect to the reference coordinate system { w }^wR_wmTAnd (4) determining. Variation of attitude of main operator relative to reference coordinate system w^wR_H(t0)-HMay be based on a previous pose of the handle (e.g., at time t 0) with respect to a reference coordinate system w^wR_H(t0)And the current pose of the handle relative to the reference frame { w } (e.g., at time t1)^wR_HAnd (4) determining. With particular reference to equation (9),

^wR_wmT(^wR_wmS)^T＝^wR_H(^wR_H(t0))^T (9)

in some embodiments, the same matrix is multiplied on each of the left and right sides of equation (9)^TbR_w(^TbR_w)^TObtaining a formula (10) based on the formula (9),

^TbR_w ^wR_wmT(^wR_wmS)^T(^TbR_w)^T＝^TbR_w ^wR_H(^wR_H(t0))^T(^TbR_w)^T (10)

equation (11) is derived based on the left side of equation (10),

^TbR_w ^wR_wmT(^wR_wmS)^T(^TbR_w)^T＝(^TbR_w ^wR_wmT)(^TbR_w ^wR_wmS)^T＝^TbR_wmT(^TbR_wmS)^T (11)

based on the right side of equation (10) results in equation (12),

^TbR_w ^wR_H(^wR_H(t0))^T(^TbR_w)^T＝^TbR_H(^TbR_H(t0))^T＝(^TbR_CombX ^CombXR_H)(^TbR_CombX ^CombXR_H(t0))^T (12)

by integrating the formulas (8) to (12), the target posture of the end of the actuator arm during the teleoperation can be obtained^TbR_wmTThe expression is as in formula (13),

^TbR_wmT＝^TbR_CombX(^CombXR_H(^CombXR_H(t0))^T)^CombXR_Tb ^TbR_wmS (13)

based on equation (13), in some embodiments, the previous pose of the handle with respect to the host operator base coordinate system { CombX } may be based^CombXR_H(t0)And current attitude^CombXR_HThe starting posture of the end of the execution arm relative to the execution arm base coordinate system { Tb }^wR_wmSAnd the transformation relationship between the base coordinate system of the execution arm { Tb } and the base coordinate system of the main operator { CombX }^CombXR_TbDetermining a target pose of the tip of the actuator arm relative to the actuator arm base coordinate system { Tb }^TbR_wmT。

In some embodimentsThe transformation relationship between the base coordinate system of the execution arm { Tb } and the base coordinate system of the main operator { CombX }^CombXR_TbMay be based on a transformation relationship between the base coordinate system of the execution arm { Tb } and the camera coordinate system { lens }^lensR_TbThe transformation relation between the camera coordinate system { lens } and the display coordinate system { Screen }^ScreenR_lensThe transformation relationship between the display coordinate system { Screen } and the main operator base coordinate system { CombX }^CombXR_ScreenAnd (4) determining.

In some embodiments, the transformation relationship between the main operator and the display may be predetermined, for example, the main operator and the display may be respectively and fixedly disposed on the main control trolley, and the display coordinate system { Screen } has a predetermined transformation relationship with the main operator base coordinate system { CombX }. In some embodiments, the execution arm base coordinate system { Tb } has a predetermined transformation relationship with the camera coordinate system { lens }. In some embodiments, the camera may be located at the end of the vision tool, and the vision tool may have finished moving before the operator performs the task, performing the transformation of the arm base coordinate system { Tb } to the camera coordinate system { lens }^lensR_TbNo longer changed.

In some embodiments, the display coordinate system { Screen } is consistent with the definition of the camera coordinate system { lens } for the field of view direction. The amount of change in the position of the image of the tip of the actuator arm in the display relative to the display coordinate system { Screen } corresponds to the amount of change in the position of the tip of the actuator arm relative to the camera coordinate system { lens }. In this way, when the operator operates by holding the handle of the main operator, the change in the attitude of the image of the actuator at the end of the actuator arm felt by the operator and the change in the attitude of the handle of the main operator felt by the operator are kept in a preset transformation relationship.

In some embodiments, the target pose of the tip of the actuator arm relative to the reference coordinate system { w } may be based on the target pose of the tip of the actuator arm relative to the actuator arm base coordinate system { Tb } and the transformation relationship of the actuator arm base coordinate system { Tb } to the reference coordinate system { w }^wR_TbAnd (4) determining. In some embodiments, the execution arm base coordinate system { Tb } has a predetermined transformation relationship with the reference coordinate system { w }. As shown in the formula (14),

one skilled in the art will appreciate that the implementation arm base coordinate system Tb can be used as the reference coordinate system w.

In some embodiments, a plurality of markers are distributed on the effector arm (e.g., on the effector arm tip 231). In some embodiments, a plurality of markings are provided on the outer surface of the cylindrical portion of the actuator arm 230. For example, a plurality of markers are circumferentially distributed on the actuator arm tip 231, such as circumferentially disposed on the outer surface of the cylindrical portion of the actuator arm tip 231. In some embodiments, the outer surface of the post portion of the effector arm tip 231 is provided with a positioning tag 232 comprising a plurality of markers, which may include a plurality of pose markers for identifying pose and at least one composite marker for identifying pose and angle (e.g., pivot angle or roll angle). In some embodiments, a positioning tag (e.g., tag 600 shown in fig. 6) is disposed on an outer surface of the column portion at the distal end of the actuator arm, and the plurality of markers may include a plurality of marker patterns distributed on the positioning tag along a circumferential direction of the column portion and a plurality of marker pattern corner points in the marker patterns. The plurality of marker patterns includes a plurality of different composite marker patterns and a plurality of pose marker patterns, which may be identical. The composite identification pattern and the pattern corner points therein can be used for identifying poses and angles, and the pose identification pattern and the pattern corner points therein can be used for identifying poses. In some embodiments, the plurality of different composite marker patterns and the plurality of pose marker patterns are located in the same pattern distribution zone, as shown in fig. 6 or 7. In some embodiments, the N continuous identification patterns in the plurality of identification patterns comprise at least one composite identification pattern, wherein N is more than or equal to 2 and less than or equal to 4, and the composite identification pattern in the N continuous identification patterns is different from the pose identification pattern. For example, a plurality of marker patterns may be uniformly distributed on the outer surface of the columnar portion, and a plurality of composite marker patterns may be uniformly distributed at intervals among the plurality of pose marker patterns, for example, one composite marker pattern is inserted every 3 pose marker patterns, as shown in fig. 6.

In some embodiments, the identification pattern may be provided on a label on the end of the actuator arm, or may be printed on the end of the actuator arm, or may be a pattern formed by the physical configuration of the actuator arm end itself, e.g., may include depressions or protrusions, and combinations thereof. In some embodiments, the identification pattern may include a pattern formed in brightness, grayscale, color, and the like. In some embodiments, the identification pattern may include a pattern that actively (e.g., self-illuminating) or passively (e.g., reflected light) provides information that is detected by the image capture device. Those skilled in the art will appreciate that in some embodiments, the pose of the marker or the pose of the marker pattern may be represented by the pose of the marker pattern corner point coordinate system. In some embodiments, the identification pattern is provided on the distal end of the effector arm in an area suitable for image acquisition by the image acquisition device, for example, an area that may be covered by the field of view of the image acquisition device during operation or an area that is not easily disturbed or obstructed during operation.

Fig. 6 illustrates a schematic diagram of a tag 600 including multiple identifiers according to some embodiments. Fig. 7 shows a schematic view of a label 700 disposed on the periphery of the distal end of the actuator arm and formed in a cylindrical shape. It will be appreciated that for simplicity, label 600 may include the same identification pattern as label 700.

Referring to fig. 6, the plurality of markers includes a plurality of pose marker patterns 610 and a plurality of pose marker pattern corner points P therein₆₁₀And a composite logo 620 and a composite logo corner R therein₆₂₀. In some embodiments, as shown in fig. 6, multiple pose identification patterns 610 and composite identification patterns 620 are disposed in the same pattern distribution zone. In the present disclosure, the pose marker pattern corner points are represented by "good" symbols, and the composite marker pattern corner points are represented by "Δ" symbols. In some embodiments, the pose identification pattern 610 or the pose identification pattern corner point P may be identified₆₁₀Determining the pose identification by identifying the composite identification pattern 620 or the corner R of the composite identification pattern₆₂₀A composite identity is determined.

Referring to fig. 7, in the circumferentially disposed state, the label 600 becomes a label 700 spatially configured in a cylindrical shape. In some embodiments, the axial angle or roll angle of each marker may be represented by the axial angle of a marker pattern or a marker pattern corner point, wherein the marker pattern comprises the pose marker pattern 710 and the composite marker pattern 720. The angle about the axis of each identification pattern or identification pattern corner mark is known or predetermined. In some embodiments, the identified angle about the axis of each marker may be determined based on the distribution of multiple markers (marker patterns or marker pattern corner points). In some embodiments, the plurality of markers may be uniformly distributed (e.g., an equidistant distribution of marker pattern corners in label 600, an equidistant distribution of marker pattern corners in label 700, and an angular distribution). In some embodiments, each identifier may be used to identify a particular angle-around-axis based on a distribution of the plurality of identifiers, each identifier having a one-to-one correspondence with the identified angle-around-axis. In this disclosure, the about-axis angle or roll angle refers to an angle about a Z-axis (e.g., the Z-axis of the tip coordinate system or the identification coordinate system of the effector arm). In some embodiments, the Z-axis may be tangential to the end of the actuator arm.

As shown in fig. 7, in the label 700, the plurality of identification patterns are uniformly distributed along the circumference of the cylindrical structure, and the plurality of identification pattern corner points are uniformly distributed on the cross-section circle 730, so that the distribution angle (for example, the angle α) of any adjacent identification pattern corner point₀) Are equal. Marking pattern corner point P for setting X-axis direction₇₀₁，P₇₀₁As a reference corner point for marking a 0-degree angle around the shaft (marking pattern corner point P)₇₀₁The located identification pattern is used as a reference pattern), the corner points P of the identification pattern can be determined according to the corner points of any identification pattern and the corner points P of the identification pattern₇₀₁The angular point identifier of the identifier pattern determines the angle around the axis of the identifier.

In some embodiments, the corner points of the pattern are identified in a set coordinate system (e.g., the identified coordinate system [ wm0 ] ≡ [ X ] shown in fig. 7_wm0 Y_wm0 Z_wm0]^T) The axial angle identified in (1) can be determined based on the following equation (15):

α_m＝α₀(m-1) (15)

wherein alpha is_mTo selectA certain mark pattern corner point (e.g. mark pattern corner point P)₇₀₁) As the first marking pattern corner point, the angle of the mth marking pattern corner point around the shaft is determined in the clockwise direction of the cross-sectional circle 730.

In some embodiments, the plurality of pose identification patterns may be the same pattern or different patterns. In some embodiments, the plurality of composite identification patterns are different patterns, each composite identification pattern may be used to identify a particular axial angle, and each composite identification pattern has a one-to-one correspondence with the identified axial angle.

Fig. 8 illustrates a schematic diagram of an implementation scenario 800, according to some embodiments of the present disclosure. As shown in fig. 8, the effector arm 840 includes a tip 830 and a distal end effector 860, and a plurality of markers (e.g., the pose marker pattern 810 and the composite marker pattern 820) may be circumferentially disposed on the tip 830. For example, label 600 as shown in FIG. 6 is circumferentially disposed on actuator arm tip 830. A plurality of corner points of the logo pattern are distributed on a cross-sectional circle 831 of the actuator arm tip 830. In some embodiments, based on the identified identification, an identification coordinate system { wm0 }. ident [ X ] is established_wm0 Y_wm0 Z_wm0]^TThe origin of the coordinate system { wm0} is the center of the circle 831, and the X-axis direction is the origin pointing to one of the marker pattern corner points (e.g., the pattern corner point P corresponding to one of the identified pose markers₈₀₁) The direction of the Z axis is parallel to the axial direction of the actuator arm tip 830 and the Y axis is perpendicular to the XZ plane.

In some embodiments, an end coordinate system [ wm }. ident [ X ] of the execution arm is established based on a plurality of composite identifications_wm Y_wmZ_wm]^TThe origin of the coordinate system { wm } at the end of the execution arm is the center of the cross-section circle 831, and the X-axis points to the corner R of the composite marking pattern₈₀₁The Z axis is parallel to or coincident with the axial direction of the actuator arm tip 830 and the Y axis is perpendicular to the XZ plane. In some embodiments, the distribution of the plurality of composite marker patterns may be based on, for example, the remaining composite marker patterns and the composite marker pattern corner points R₈₀₁And determining the angle of the composite identification pattern corner point mark contained in the composite identification pattern around the shaft according to the position relation of the corresponding composite identification pattern.

With continued reference to FIG. 3, at step 303, a positioning image is acquired. In some embodiments, the positioning image includes a plurality of markers on the distal end of the effector arm. In some embodiments, the plurality of markers includes a plurality of pose markers for identifying a pose and at least one composite marker for identifying a pose and an angle. In some embodiments, the positioning image may be received from an image acquisition device 250 as shown in FIG. 2. For example, the control device 220 may receive positioning images actively transmitted by the image acquisition apparatus 250. Alternatively, the control device 220 may send an image request instruction to the image capturing apparatus 250, and the image capturing apparatus 250 sends the positioning image to the control device 220 in response to the image request instruction.

With continued reference to FIG. 3, in step 305, a plurality of markers located on the distal end of the effector arm are identified in the positioning image. For example, an exemplary method of identifying a plurality of identifiers located on a distal end of an effector arm may include the method shown in fig. 11 and 13. In some embodiments, the control device 220 may identify some or all of the identifiers in the positioning image through an image processing algorithm. In some embodiments, the image processing algorithm may include a feature recognition algorithm, which may extract or recognize the identified features. For example, the image processing algorithm may comprise a corner detection algorithm for detecting the corner of the identification pattern. The corner detection algorithm may be one of, but not limited to, a gray-scale image-based corner detection, a binary image-based corner detection, and a contour curve-based corner detection. For example, the image processing algorithm may be a color feature extraction algorithm for detecting color features in the identification pattern. As another example, the image processing algorithm may be a contour detection algorithm for detecting contour features of the identification pattern. In some embodiments, the control device may identify the identity of some or all of the positioning images by the recognition model.

With continued reference to FIG. 3, at step 307, an actual pose of the tip of the effector arm is determined based on the at least one composite signature and the plurality of pose signatures. In some embodiments, the pose of the end coordinate system of the effector arm relative to the reference coordinate system may be determined as the actual pose of the end of the effector arm based on the two-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the positioning image and the three-dimensional coordinates in the end coordinate system of the effector arm.

In some embodiments, the method 300 may further include determining a plurality of two-dimensional coordinates identified in the positioning image. In some embodiments, the coordinates of the marker may be represented by coordinates of the corner points of the marker pattern. For example, two-dimensional coordinates identified in the positioning image and three-dimensional coordinates identified in the tip coordinate system of the effector arm may be represented by coordinates identifying corner points of the pattern. In some embodiments, determining the two-dimensional coordinates of the plurality of markers in the positioning image may include determining the two-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the positioning image. In some embodiments, the method 500 may further include determining three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the tip coordinate system of the effector arm based on the at least one composite marker.

In some embodiments, the method 300 may further include determining a pose of the tip coordinate system of the effector arm relative to the reference coordinate system based on two-dimensional coordinates of the at least one composite marker pattern corner point and the plurality of pose marker pattern corner points in the positioning image and a transformation relationship of the three-dimensional coordinates in the tip coordinate system of the effector arm and the camera coordinate system relative to the reference coordinate system. In some embodiments, the transformation relationship of the camera coordinate system relative to the reference coordinate system may be known. For example, the reference coordinate system is a world coordinate system, and the transformation relationship of the camera coordinate system relative to the world coordinate system can be determined according to the pose of the camera. In other embodiments, the reference coordinate system may be the camera coordinate system itself according to actual requirements. In some embodiments, the pose of the tip coordinate system of the effector arm relative to the camera coordinate system is determined based on the two-dimensional coordinates of the at least one composite marker pattern corner point and the plurality of pose marker pattern corner points in the positioning image and the three-dimensional coordinates in the tip coordinate system of the effector arm based on the camera imaging principles and the projection model. Based on the pose of the terminal coordinate system of the actuator arm relative to the camera coordinate system and the transformation relationship of the camera coordinate system relative to the reference coordinate system, the pose of the terminal coordinate system of the actuator arm relative to the reference coordinate system can be obtained.

In some embodiments, the internal parameters of the camera may also be considered. For example, the internal reference of the camera may be the internal reference of the camera of the image capture device 250 as shown in fig. 2. The internal parameters of the camera may be known or calibrated. In some embodiments, the camera coordinate system may be understood as a coordinate system established with the origin of the camera. For example, a coordinate system established with the optical center of the camera as the origin or a coordinate system established with the lens center of the camera as the origin. When the camera is a binocular camera, the origin of the camera coordinate system may be the center of the left lens of the camera, or the center of the right lens, or any point on the line connecting the centers of the left and right lenses (e.g., the midpoint of the line).

In some embodiments, the pose of the tip coordinate system { wm } of the effector arm relative to the reference coordinate system (e.g., world coordinate system) { w } may be determined based on equation (16) below:

^wR_wm＝^wR_lens ^lensR_wm

^wP_wm＝^wR_lens(^lensR_wm+^lensP_wm)+^wP_lens (16)

wherein the content of the first and second substances,^wR_wmto perform the pose of the end coordinate system wm of the arm with respect to the reference coordinate system,^wP_wmto perform the position of the end coordinate system of the arm relative to the reference coordinate system,^wR_lensis the pose of the camera coordinate system relative to the reference coordinate system,^wP_lensis the position of the camera coordinate system relative to the reference coordinate system,^lensR_wmto perform the pose of the end coordinate system of the arm relative to the camera coordinate system,^lensP_wmis the position of the coordinate system of the end of the manipulator arm relative to the camera coordinate system.

Fig. 9 illustrates a flow diagram of a method 900 of determining an actual pose of an end of an effector arm according to some embodiments of the present disclosure. As shown in fig. 9, some or all of the steps of the method 900 may be performed by a control device (e.g., the control device 120 shown in fig. 1, the control device 220 shown in fig. 2, or the control device 1620 shown in fig. 16). Some or all of the steps of method 900 may be implemented by software, firmware, and/or hardware. In some embodiments, method 900 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1 or robotic system 1600 shown in fig. 16). In some embodiments, method 900 may be implemented as computer readable instructions. These instructions may be read and executed by a general-purpose processor or a special-purpose processor, such as control 1620 shown in fig. 16. In some embodiments, these instructions may be stored on a computer-readable medium.

Referring to fig. 9, in step 901, three-dimensional coordinates of at least one composite marker and a plurality of pose markers in a marker coordinate system are determined. In some embodiments, the three-dimensional coordinates of each marker pattern corner point in the marker coordinate system { wm0} may be determined based on equation (17) below:

C_m＝[r·cosα_m r·sinα_m 0]^T (17)

wherein, C_mTo use the selected corner point of the identification pattern as the first corner point of the identification pattern (e.g., the corner point P of the pose identification pattern)₈₀₁) According to the clockwise direction of the cross-section circle 831, the three-dimensional coordinate of the mth marking pattern corner point in the marking coordinate system, and r is the radius.

In some embodiments, the determination of the axial angle α identified by the mth identification pattern angle point is based on equation (15)_mAnd then the angle α around the shaft determined based on equation (15)_mAnd equation (17) determines the three-dimensional coordinate C of the mth marker pattern corner point in the marker coordinate system { wm0}_m。

Referring to fig. 9, at step 903, a roll angle of the landmark coordinate system relative to the tip coordinate system of the effector arm is determined based on the at least one composite landmark. In some embodiments, a first-axis angle identified by one of the at least one composite marker in the tip coordinate system of the effector arm may be determined, and a second-axis angle identified by the composite marker in the marker coordinate system may be determined. Based on the first and second axial angles, a roll angle of the identification coordinate system relative to the tip coordinate system of the effector arm may be determined. In some embodiments, referring to FIG. 8, roll angle Δ α may refer to the angle of rotation about the Z axis of the identified coordinate system { wm0} relative to the end coordinate system { wm } of the actuator arm. In some embodiments, the roll angle Δ α may be determined based on the following equation (18):

Δα＝α₁-α₂ (18)

wherein alpha is₁Is a first axial angle, α₂Is a second axial angle. The first axial angle is a composite marker pattern corner point (e.g., composite marker pattern corner point R)₈₀₂) An on-axis angle identified in the end coordinate system of the actuator arm, the second on-axis angle being a composite identification pattern corner point (e.g., composite identification pattern corner point R)₈₀₂) The identified angle about the axis in the identified coordinate system.

In some embodiments, the X-axis of the marker coordinate system { wm0} points to a composite marker pattern corner point (e.g., composite marker pattern corner point R₈₀₂) The method 900 may further include determining a first-axis angle of the composite marker identified in the tip coordinate system of the effector arm as a roll angle of the marker coordinate system relative to the tip coordinate system of the effector arm. In some embodiments, the first-axis angle may be determined based on a pattern included with the composite mark.

Referring to fig. 9, at step 905, three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the tip coordinate system of the effector arm are determined based on the roll angle of the marker coordinate system relative to the tip coordinate system of the effector arm and the three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the marker coordinate system. It will be appreciated that given the roll angle of the tag coordinate system relative to the tip coordinate system of the effector arm, the three-dimensional coordinates of a plurality of tag pattern corner points (e.g., composite tag pattern corner points and pose tag pattern corner points) in the tag coordinate system may be transformed into three-dimensional coordinates in the tip coordinate system of the effector arm according to a coordinate transformation.

Referring to fig. 9, in step 907, based on the two-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the positioning image and the three-dimensional coordinates in the tip coordinate system of the effector arm, the pose of the tip coordinate system of the effector arm with respect to the reference coordinate system is determined as the actual pose of the tip of the effector arm. In some embodiments, step 907 of method 900 may be implemented similarly to determining the actual pose of the tip of the effector arm in method 300.

FIG. 10 illustrates a flow chart of a method 1000 of determining an actual pose of an end of an effector arm according to further embodiments of the present disclosure. Method 1000 may be an alternative embodiment of method 900 of fig. 9. As shown in fig. 10, some or all of the steps of the method 1000 may be performed by a control device (e.g., the control device 120 shown in fig. 1, the control device 220 shown in fig. 2, or the control device 1620 shown in fig. 16). Some or all of the steps of method 1000 may be implemented by software, firmware, and/or hardware. In some embodiments, method 1000 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1 or robotic system 1600 shown in fig. 16). In some embodiments, method 1000 may be implemented as computer-readable instructions. These instructions may be read and executed by a general-purpose processor or a special-purpose processor, such as control 1620 shown in fig. 16. In some embodiments, these instructions may be stored on a computer-readable medium.

Referring to fig. 10, in step 1001, the pose of the marker coordinate system with respect to the reference coordinate system is determined based on the two-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the positioning image and the three-dimensional coordinates in the marker coordinate system. In some embodiments, the three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the marker coordinate system may be implemented similarly to step 901 of method 900.

Referring to FIG. 10, at step 1003, a roll angle of the landmark coordinate system relative to the tip coordinate system of the effector arm is determined based on the at least one composite landmark. Determining the roll angle of the identification coordinate system relative to the tip coordinate system of the effector arm may be accomplished in some embodiments similarly to step 903 in method 900.

Referring to fig. 10, in step 1005, the pose of the tip coordinate system of the effector arm with respect to the reference coordinate system is determined as the actual pose of the tip of the effector arm based on the roll angle of the identifier coordinate system with respect to the tip coordinate system of the effector arm and the pose of the identifier coordinate system with respect to the reference coordinate system.

For example, the pose of the tip coordinate system { wm } of the effector arm relative to the reference coordinate system (e.g., world coordinate system) { w } may be determined based on equation (19) below:

wherein the content of the first and second substances,^wR_wmto perform the pose of the end coordinate system of the arm with respect to the reference coordinate system,^wP_wmto perform the position of the end coordinate system of the arm relative to the reference coordinate system,^wR_wm0to identify the pose of the coordinate system relative to the reference coordinate system,^wP_wm0to identify the position of the coordinate system relative to the reference coordinate system, rot_z(Δ α) represents the roll angle Δ α of rotation about the Z-axis of the tip coordinate system of the effector arm.

Fig. 11 illustrates a flow diagram of a method 1100 for identifying an identity, according to some embodiments of the present disclosure. As shown in fig. 11, some or all of the steps of the method 1100 may be performed by a control device (e.g., the control device 120 shown in fig. 1, the control device 220 shown in fig. 2, or the control device 1620 shown in fig. 16). Some or all of the steps of method 1100 may be implemented by software, firmware, and/or hardware. In some embodiments, method 1100 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1 or robotic system 1600 shown in fig. 16). In some embodiments, the method 1100 may be implemented as computer-readable instructions. These instructions may be read and executed by a general-purpose processor or a special-purpose processor, such as control 1620 shown in fig. 16. In some embodiments, these instructions may be stored on a computer-readable medium.

Referring to fig. 11, in step 1101, a plurality of candidate identifications are determined from the scout image. In some embodiments, the marker may comprise marker pattern corner points in the marker pattern. The coordinates or coordinate system origin of the candidate markers may be represented by the candidate marker pattern corners. In some embodiments, the candidate marker pattern corner points may refer to possible marker pattern corner points obtained by performing a preliminary process or a preliminary identification on the positioning image.

In some embodiments, method 1100 may include determining a Region of Interest (ROI) in the scout image. For example, the ROI may be first cut out from the scout image, and a plurality of candidate identifications may be determined from the ROI. The ROI may be a full image of the positioning image or a partial region. For example, the ROI of the current frame may be truncated based on a region within a certain range of the corner points of the plurality of identification patterns determined from the previous frame of image (e.g., the positioning image of the previous image processing cycle). For the positioning image of the non-first frame, the ROI may be a region within a certain distance range centered on an imaginary point formed by coordinates of a plurality of corner points of the identification pattern in the previous image processing cycle. The range of distances may be a fixed multiple, e.g. twice, of the average separation distance of the corner points of the identification pattern. It should be understood that the predetermined multiple may also be a variable multiple of the average spacing distance of the corner points of the plurality of candidate marker patterns in the previous image processing cycle.

In some embodiments, the method 1100 may include determining Corner Likelihood values (CL) for locating each pixel point in the image. In some embodiments, the corner likelihood value of a pixel point may be a numerical value characterizing the likelihood of the pixel point as a feature point (e.g., a corner). In some embodiments, the positioning image may be preprocessed before calculating the corner likelihood value of each pixel point, and then the corner likelihood value of each pixel point in the preprocessed image is determined. The pre-processing of the image may include, for example: at least one of image graying, image denoising and image enhancement. For example, image pre-processing may include: and intercepting the ROI from the positioning image, and converting the ROI into a corresponding gray image.

In some embodiments, the manner of determining the corner likelihood value of each pixel point in the ROI may include, for example, performing a convolution operation on each pixel point within the ROI to obtain a first and/or second derivative of each pixel point. And solving the corner likelihood value of each pixel point by using the first-order and/or second-order derivative of each pixel point in the ROI. Illustratively, the corner likelihood value of each pixel may be determined based on the following formula (20):

CL＝max(c_xy,c₄₅)

wherein τ is a set constant, for example set to 2; I.C. A_x、I₄₅、I_y、I_n45Respectively the first derivatives of the pixel points in four directions of 0, pi/4, pi/2 and-pi/4; i is_xyAnd I_{45_45}The second derivatives of the pixel points in the directions of 0, pi/2 and pi/4, -pi/4, respectively.

In some embodiments, the method 1100 may include dividing the ROI into a plurality of sub-regions. For example, a non-maximum suppression method may be used to equally divide multiple sub-images in a ROI region. In some embodiments, the ROI may be equally divided into a plurality of sub-images of 5 x 5 pixels. The above embodiments are exemplary and not limiting, it being understood that the positioning image or ROI may also be segmented into sub-images of other sizes, for example, into sub-images of 9 x 9 pixels.

In some embodiments, the method 1100 may include determining the pixel in each sub-region for which the corner likelihood value is the greatest to form a set of pixels. For example, the pixel point with the largest CL value in each sub-image may be determined, the pixel point with the largest CL value in each sub-image may be compared with the first threshold, and the set of pixels with CL values larger than the first threshold may be determined. In some embodiments, the first threshold may be set to 0.06. It should be understood that the first threshold may also be set to other values.

Referring to FIG. 11, in step 1103, a first token of the plurality of tokens is identified from the plurality of candidate tokens. In some embodiments, the first marker is identified based on the marker pattern matching template. In some embodiments, the marker pattern matching templates include at least one pose marker pattern matching template that is a composite marker pattern matching template that is different from the plurality of patterns. In some embodiments, the composite signature is identified based on a plurality of composite signature pattern matching templates that differ in pattern. For example, under the condition that the identification patterns of the pose identification are the same, the pose identification pattern matching template can be matched with the candidate identification, and if the matching fails, a plurality of different composite identification pattern matching templates are matched with the candidate identification one by one until the matching is successful.

In some embodiments, the first marker is identified using a marker pattern matching template to match the pattern at the corner points of the candidate marker pattern. For example, the candidate marker pattern corner point reaching the preset pose pattern matching degree standard is determined as a first marker pattern corner point. In some embodiments, the identification pattern matching template has the same or similar features as the pattern in the region near the corner of the identification pattern. If the matching degree between the identification pattern matching template and the patterns in the areas near the candidate identification pattern corners reaches a preset pattern matching degree standard (for example, the matching degree is higher than a threshold), the patterns in the areas near the candidate identification pattern corners and the identification pattern matching template can be considered to have the same or similar characteristics, and then the current candidate identification pattern corners can be considered as the identification pattern corners.

In some embodiments, the pixel point with the largest CL value in the pixel set is determined as the candidate identification pattern corner point. For example, all pixels in the pixel set may be sorted in order of CL value from large to small, and the pixel with the largest CL value may be used as the candidate identification pattern corner. In some embodiments, after determining the candidate marker pattern corner, matching the pattern at the candidate marker pattern corner using the marker pattern matching template, and if a preset pattern matching degree criterion is reached, determining the candidate marker pattern corner as the identified first marker pattern corner.

In some embodiments, the method 1100 may further include, in response to a failure to match, determining a pixel of the set of pixels having a largest corner likelihood value as the candidate identification pattern corner. For example, if the candidate logo pattern corner does not meet the preset matching degree standard, selecting a pixel point of the secondary CL value (a pixel point with the second largest CL value) as the candidate logo pattern corner, matching the pattern at the candidate logo pattern corner by using the logo pattern matching template, and repeating the steps until the first logo pattern corner is identified.

In some embodiments, the identification pattern mayIs a checkerboard pattern with alternate black and white, so the identification pattern matching template can be the same checkerboard pattern, and the gray distribution G of the identification pattern matching template is utilized_MPixel neighborhood gray distribution G of pixel point corresponding to candidate identification pattern corner point_imageThe Correlation Coefficient (CC) between the two signals. Pixel neighborhood gray distribution G of pixel points_imageThe gray scale distribution of pixels in a certain range (for example, 10 × 10 pixels) centered on the pixel point. The correlation coefficient may be determined based on the following equation (21):

where Var () is a variance function and Cov () is a covariance function. In some embodiments, when the correlation coefficient is less than 0.8, and the correlation between the gray scale distribution in the pixel domain and the matching template of the identification pattern is low, it is determined that the candidate identification pattern corner with the largest corner likelihood value is not the identification pattern corner, otherwise, it is determined that the candidate identification pattern corner with the largest corner likelihood value is the identification pattern corner.

In some embodiments, the method 1100 may further include determining edge directions of candidate logo pattern corners. For example, as shown in fig. 12, the candidate pose identification pattern corner point is the corner point P in the pose identification pattern 1200₁₂₀₁Then the corner point P₁₂₀₁The edge direction of (a) may refer to the formation of a corner point P₁₂₀₁As indicated by the dashed arrows in fig. 12.

In some embodiments, the edge direction may be determined by taking the first derivative values (I) of each pixel of a range of neighborhoods (e.g., 10 × 10 pixels) centered on the corner of the candidate logo pattern in the X and Y directions of the planar coordinate system_xAnd I_y) And (4) determining. For example, the edge direction may be determined based on the following equation (22):

wherein the first derivative (I)_xAnd I_y) The method can be obtained by performing convolution operation on each pixel point in a certain range of neighborhood. In some embodiments, the edge direction I is determined by calculating the edge direction of the pixels in each range neighborhood_angleAnd corresponding weight I_weightClustering to obtain edge direction of the pixel point, and selecting weight I_weightI corresponding to the largest class_angleAs the edge direction. It should be noted that if there are multiple edge directions, the weight I is selected_weightI corresponding to a plurality of classes with the largest proportion_angleAs the edge direction.

In some embodiments, the method used for the Clustering calculation may be any one of a K-means method, a BIRCH (Balanced Iterative Clustering method Based on hierarchical structure) method, a DBSCAN (Density-Based Clustering method with Noise) method, a GMM (Gaussian Mixed Model) method.

In some embodiments, method 1100 may include identifying pattern matching templates based on edge direction rotation. Rotating the logo pattern matching template based on the edge direction may align the logo pattern matching template with the image at the candidate logo pattern corner points. The edge direction of the candidate marker pattern corner point can be used to determine the setting direction of the image at the candidate marker pattern corner point in the positioning image. In some embodiments, the marker pattern matching template is rotated based on the edge direction, and the marker pattern matching template may be adjusted to be the same or nearly the same as the image direction at the corner point of the candidate marker pattern to facilitate image matching.

Referring to fig. 11, in step 1105, with the first identifier as a starting point, other identifiers are searched for. In some embodiments, in response to identifying the composite marker, other markers are identified based on the pose marker pattern matching template. In some embodiments, the other markers include pose markers or composite markers.

Fig. 13 illustrates a flow diagram of a method 1300 for searching for an identity, according to some embodiments of the present disclosure. As shown in fig. 13, some or all of the steps in the method 1300 may be performed by a control device (e.g., the control device 120 shown in fig. 1, the control device 220 shown in fig. 2, or the control device 1620 shown in fig. 16). Some or all of the steps in method 1300 may be implemented by software, firmware, and/or hardware. In some embodiments, method 1300 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1 or robotic system 1600 shown in fig. 16). In some embodiments, method 1300 may be implemented as computer-readable instructions. These instructions may be read and executed by a general-purpose processor or a special-purpose processor, such as control 1620 shown in fig. 16. In some embodiments, these instructions may be stored on a computer-readable medium.

Referring to fig. 13, in step 1301, a second identity is determined, starting with a first identity. In some embodiments, the second marker pattern corner point is searched in the set search direction with the first marker pattern corner point as a starting point. In some embodiments, the set search direction may include at least one of a direction directly in front of (corresponding to a 0 ° angular direction), directly behind (corresponding to a 120 ° angular direction), directly above (a 90 ° angular direction), directly below (-a 90 ° angular direction), and obliquely (e.g., ± 45 ° angular directions) the corner point of the first identification pattern.

In some embodiments, n search directions are set, for example, the search is performed in 8 directions, each search direction v_snMay be determined based on the following equation (23):

v_sn＝[cos(n·π/4)sin(n·π/4)],(n＝1,2,…,8) (23)

in some embodiments, the search direction set in the current step may be determined according to a deviation angle between adjacent identification pattern corner points in the plurality of identification pattern corner points determined in the previous frame. Illustratively, the predetermined search direction may be determined based on the following formula (24):

wherein (x)_j,y_j) Two-dimensional coordinates of a plurality of identification pattern corner points determined for a previous frame (or a previous image processing cycle); n is_lastDetermining the number of a plurality of identification pattern corner points for the previous frame; v. of_s1A search direction set for the first; v. of_s2For the second set search direction.

In some embodiments, as shown in FIG. 14, the corner point P is marked with a first marker pattern₁₄₀₁Is used as a search starting point, and a second identification pattern corner point P is searched in the set search direction₁₄₀₂The coordinate position of (a). For example, with a first marking pattern corner point P₁₄₀₁Is used as a search starting point, and is searched in a set search direction V by a search frame (for example, a dashed line frame in fig. 14) at a constant search step length₁₄₀₁And searching for corner points of the identification pattern.

In some embodiments, if at least one candidate identifier exists in the search box, the candidate identifier pattern corner point with the maximum corner point likelihood value in the search box is preferentially selected as the second identifier pattern corner point P₁₄₀₂. With the corner point P of the first identification pattern limited to a suitable size₁₄₀₁Is used as a search starting point to perform second marking of the pattern corner point P₁₄₀₂During searching, the candidate identification pattern corner with the maximum corner likelihood value in the candidate identifications appearing in the search box has higher possibility of being the identification pattern corner. Therefore, the candidate marker with the largest corner likelihood value in the search box can be regarded as the second marker pattern corner P₁₄₀₂In order to increase the data processing speed. In other embodiments, in order to improve the accuracy of identifying the corner points of the identification pattern, the candidate identification pattern corner point with the largest corner likelihood value among the candidate identifications appearing in the search box is selected to identify the corner point, so as to determine whether the candidate identification pattern corner point with the largest corner likelihood value is the identification pattern corner point. For example, a pose identification pattern matching template or a composite identification pattern matching template may be used to match images within a certain range at the corner of the candidate identification pattern with the maximum likelihood, and the candidate identification pattern corner meeting the preset pattern matching degree criterion may be regarded as the searched second identification pattern corner P₁₄₀₂。

In some embodiments, with continued reference to FIG. 14, the size of the search box may be increased in steps, such that the search range is increased in steps. The search step size may vary synchronously with the side length of the search box. In other embodiments, the size of the search box may be fixed.

In some embodiments, the marker pattern may be a black and white alternating pattern, and the pattern matching may be performed based on the correlation coefficient in equation (21). And if the correlation coefficient is larger than the threshold value, the candidate identification pattern corner with the largest corner likelihood value is regarded as the identification pattern corner and is marked as a second identification pattern corner.

Referring to fig. 13, in step 1303, a search direction is determined based on the first identifier and the second identifier. In some embodiments, the search direction comprises: a first search direction and a second search direction. The first search direction may be a direction starting from the coordinate position of the first identification pattern corner point and away from the second identification pattern corner point. The second search direction may be a direction starting from the coordinate position of the second identification pattern corner point and away from the first identification pattern corner point. For example, the search direction V shown in FIG. 14₁₄₀₂。

In step 1305, the identifiers are searched in the search direction with the first identifier or the second identifier as a starting point. In some embodiments, if the first identification pattern corner point is taken as a new starting point, the first search direction in the above embodiments may be taken as a search direction to perform the search for the identification pattern corner point. If the second identification pattern corner point is used as a new search starting point, the second search direction in the above embodiment may be used as a search direction to search for the identification pattern corner point. In some embodiments, a new identification pattern corner point (e.g., the third identification pattern corner point P in FIG. 14) is searched for₁₄₀₃) May be performed similarly to step 1301. In some embodiments, the search step size may be the first identification pattern corner point P₁₄₀₁And a second identification pattern corner point P₁₄₀₂Distance L between₁。

In some embodiments, in response to the search distance being greater than the search distance threshold, determining a pixel with the largest corner likelihood value of the remaining pixels in the pixel set as a candidate identification pattern corner; and matching the identification pattern matching template with the identification patterns at the positions of the corner points of the candidate identification patterns to identify the first identification. In some embodiments, after determining the pixel with the largest corner likelihood value of the remaining pixels in the pixel set as the new candidate marker pattern corner, the new first marker may be identified based on a method similar to step 1103. In some embodiments, a search distance greater than a search distance threshold may be understood as a search distance greater than a search distance threshold in some or all of the search directions. In some embodiments, the search distance threshold may comprise a set multiple of the distance between the (N-1) th pose identification pattern corner point and the (N-2) th pose identification pattern corner point, where N ≧ 3. For example, the search distance threshold is twice the distance of the first two identified pattern corner points. Thus, the maximum search distance for searching the third identification pattern corner is twice the distance between the first identification pattern corner and the second identification pattern corner, if the identification pattern corner is not searched yet when the search distance is reached in the search direction, the pixel with the maximum corner likelihood value of the remaining pixels in the pixel set is determined as a new candidate pose identification pattern corner, a new first identification is identified, and the current search process is correspondingly stopped. In some embodiments, similar to method 1100, a new first marker pattern corner may be re-determined, and similar to method 1300, the remaining marker pattern corners may be searched for, with the new marker pattern corner as a search starting point.

In some embodiments, in response to the number of identified markers being greater than or equal to the marker number threshold, a pose of the tip of the implement arm relative to the reference coordinate system may be determined based on the identified markers, and the search for the markers may be stopped accordingly. For example, in response to the number of identified marker pattern corner points being greater than or equal to the marker number threshold, the search for marker pattern corner points is stopped. For example, when four marker pattern corners are identified, the search for marker pattern corners is stopped.

In some embodiments, in response to the number of identified markers being less than the marker number threshold, determining a pixel of the pixel set having the largest corner likelihood value of the remaining pixels as a candidate marker pattern corner; and matching the identification pattern matching template with the identification patterns at the positions of the corner points of the candidate identification patterns to identify the first identification. In some embodiments, if the total number of identified corner points of the marker pattern is less than the threshold of the number of markers, the search based on the first marker pattern in the above steps is considered to fail. In some embodiments, if all the identified identifiers do not include the composite identifier, for example, the identified identifier pattern corner points do not include the composite identifier pattern corner points, the search based on the first identifier pattern in the above step is considered to have failed. In some embodiments, in case of a search failure, the pixel with the largest corner likelihood value of the remaining pixels in the pixel set is determined as a new candidate identification pattern corner, after which a new first identification may be identified based on a method similar to step 1103. In some embodiments, similar to method 1100, a new first marker pattern corner may be re-determined, and similar to method 1300, the remaining marker pattern corners may be searched for, with the new marker pattern corner as a search starting point.

In some embodiments, if the identified markers include composite markers, the remaining markers searched may not determine the marker type (it is understood that the marker type includes pose markers and composite markers). For example, if the first identifier is a composite identifier, it may be uncertain whether the second identifier is specifically a pose identifier or a composite identifier.

In some embodiments, if the identified identifier does not include a composite identifier, the type of the new identifier searched is determined. For example, if the first identifier is not a composite identifier, it needs to be determined whether the second identifier is specifically a pose identifier or a composite identifier. If the first identifier and the second identifier are not composite identifiers, it is required to determine whether the third identifier is a pose identifier or a composite identifier, and so on.

In some embodiments, after the identification pattern corner is searched or identified, the determined identification pattern corner may be sub-pixel positioned to improve the position accuracy of the identification pattern corner.

In some embodiments, the CL values of the pixel points may be model-based fitted to determine the coordinates of the sub-pixel located marker pattern corner points. For example, the fitting function of the CL value of each pixel point in the ROI may be a quadratic function, and the extreme point of the function is a sub-pixel point. The fitting function may be determined based on the following equations (25) and (26):

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

wherein S (x, y) is a fitting function of CL values of all pixel points in each ROI, and a, b, c, d, e and f are coefficients; x is a radical of a fluorine atom_cX-coordinate, y, for pose identification_cThe y coordinate of the pose mark.

With continued reference to FIG. 3, at step 309, a fault-related control signal is generated in response to the target pose and the actual pose satisfying the error detection condition. After obtaining the target pose and the actual pose of the tip end of the actuator arm, the control device determines a pose error of the tip end of the actuator arm to determine whether the actuator arm has correctly advanced to a position and a pose desired by an operator, and further determines whether the robot system has failed. In some embodiments, when the target pose and the actual pose of the actuator arm satisfy an error detection condition (e.g., equal to or greater than an error threshold), the control device determines that the actuator arm has not moved correctly to a position and a pose corresponding to the main operator, and issues a control signal related to a fault. For example, the control device may issue a first alarm signal indicating that the control of the actuator arm is faulty.

In some embodiments, the control device may acquire multiple sets of target poses and actual poses of the actuator arm in real time during the teleoperation, and comprehensively judge the operating condition of the actuator arm based on the multiple sets of target poses and actual poses. In some embodiments, the control device may determine the target pose and the actual pose of the end of the actuator arm at predetermined periods, perform error detection on the actuator arm through a plurality of detection cycles, analyze a plurality of sets of errors using a mathematical statistical method, and issue a control signal related to a fault when an error detection condition is satisfied.

For example, in the k-th error detection cycle, the pose difference can be expressed as follows:

wherein the content of the first and second substances,

the positional difference of the arm is performed for the k-th error detection cycle,

is the angular difference of the actuator arm at the k-th error detection cycle, P_t ^kFor the target position of the actuator arm at the k-th error detection cycle, R_t ^kFor the target pose of the arm performed at the k-th error detection cycle,

for the actual position of the actuator arm at the k-th error detection cycle, R_r ^kFor the actual pose of the arm performed at the kth error detection cycle,

to represent

And with

The angle of rotation therebetween.

In some embodiments, the control device may store the errors obtained in the plurality of detection cycles in a memory, accumulate the errors, and issue a control signal related to the fault when the accumulated value of the errors satisfies an error detection condition (e.g., exceeds a threshold).

In some embodiments, the method 300 further includes receiving state information for driving the at least one drive of the implement arm in response to the target pose and the actual pose satisfying an error detection condition, and issuing a second alarm signal indicating a failure of the drive of the implement arm in response to the state information and the drive information of the at least one drive satisfying a failure detection condition.

In some embodiments, the drive device is provided with a drive device sensor coupled to the drive device and configured to obtain status information of the drive device. For example, the drive means may comprise at least one drive motor and the drive means sensor may comprise a potentiometer or encoder, the drive means sensor being coupled to the drive motor to record and output status information of the motor. The control device sends driving information to the at least one driving device based on the target pose of the tail end of the execution arm, receives state information of the at least one driving device for driving the execution arm through the driving device sensor, and sends out a second alarm signal for indicating that the at least one driving device for driving the execution arm is in failure when the state information and the driving information meet failure detection conditions (for example, greater than or equal to an error threshold value).

In some embodiments of the present disclosure, the present disclosure also provides a computer device comprising a memory and a processor. The memory may be configured to store at least one instruction, and the processor is coupled to the memory and configured to execute the at least one instruction to perform some or all of the steps of the methods of the present disclosure, such as some or all of the steps of the methods disclosed in fig. 3, 4, 9, 10, 11, and 13.

Fig. 15 shows a schematic block diagram of a computer device 1500 according to some embodiments of the present disclosure. Referring to fig. 15, the computer device 1500 may include a Central Processing Unit (CPU)1501, a system memory 1504 including a Random Access Memory (RAM)1502 and a Read Only Memory (ROM)1503, and a system bus 1505 that connects the various components. The computer device 1500 may also include an input/output system, and a mass storage device 1507 for storing an operating system 1513, application programs 1514, and other program modules 1515. The input/output device includes an input/output controller 1510 mainly composed of a display 1508 and an input device 1509.

The mass storage device 1507 is connected to the central processing unit 1501 through a mass storage controller (not shown) connected to the system bus 1505. The mass storage device 1507 or the computer-readable medium provides non-volatile storage for the computer device. The mass storage device 1507 may include a computer-readable medium (not shown) such as a hard disk or Compact disk Read-Only Memory (CD-ROM) drive.

Without loss of generality, computer readable media may comprise computer storage media and communication media. Computer storage media includes volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. Computer storage media includes RAM, ROM, flash memory or other solid state storage technology, CD-ROM, or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices. Of course, those skilled in the art will appreciate that computer storage media is not limited to the foregoing. The system memory and mass storage devices described above may be collectively referred to as memory.

The computer device 1500 may be connected to the network 1512 through a network interface unit 1511 connected to the system bus 1505.

The system memory 1504 or mass storage device 1507 is also used to store one or more instructions. The central processor 1501 implements all or a portion of the steps of the methods in some embodiments of the present disclosure by executing the one or more instructions.

In some embodiments of the present disclosure, the present disclosure also provides a computer-readable storage medium having stored therein at least one instruction, which is executable by a processor to cause a computer to perform some or all of the steps of the methods of some embodiments of the present disclosure, such as some or all of the steps of the methods disclosed in fig. 3, 4, 9, 10, 11, and 13. Examples of computer readable storage media include memories of computer programs (instructions), such as Read-Only memories (ROMs), Random Access Memories (RAMs), Compact Disc Read-Only memories (CD-ROMs), magnetic tapes, floppy disks, and optical data storage devices, among others.

Fig. 16 shows a schematic 1600 of a robotic system according to some embodiments of the present disclosure. As shown in fig. 16, a robotic system 1600, comprising: a master operator 1610, a control device 1620, a driving device 1660, a driven tool 1650, and an image capture apparatus 1670. The main operator 1610 includes a robot arm, a handle provided on the robot arm, and at least one main operator sensor provided at least one joint on the robot arm, the at least one main operator sensor being used to obtain joint information of the at least one joint. In some embodiments, the master manipulator 1610 includes a six-degree-of-freedom robot arm, one master manipulator sensor is disposed at each joint on the six-degree-of-freedom robot arm, and joint information (e.g., joint angle data) is generated by the master manipulator sensor of each joint. In some embodiments, the master operator sensor employs a potentiometer and/or encoder. An actuator arm 1640 is provided on the driven tool 1650, and in some embodiments, the actuator arm 1640 includes a multi-segment continuous body deformable arm. A plurality of markers including a plurality of pose markers and at least one composite marker may be formed or disposed on the distal end 1630 of the actuator arm 1640, and an actuator may be disposed at the distal end of the distal end 1630. The image capture device 1670 may be used to capture positioning images of the effector arm 1640. Actuator 1660 is used to actuate actuator arm 1640, and at least one actuator sensor is coupled to the at least one actuator and used to obtain actuation information. The control device 1620, communicatively connected to the main operator 1610, the at least one driver 1660, and the image capture device 1670, is configured to perform some or all of the steps of the method according to some embodiments of the present disclosure, such as some or all of the steps of the method disclosed in fig. 3, 4, 9, 10, 11, and 13.

The robot has high requirements on operation precision and human-computer interaction experience. In the operation process of the robot system, if the execution arm cannot accurately and quickly move the target position and the target posture, the operation experience of an operator can be reduced, even operation failure can be caused, and unnecessary risks are generated. In the embodiment of the disclosure, the actual pose of the operation arm is detected and compared with the target pose of the execution arm expected by the operator in real time, so that the existing fault risk can be found. The embodiment of the disclosure can improve the operability and safety of the robot system and reduce the operation risk caused by the pose error of the execution arm in the operation process of the robot system.

It is noted that the foregoing is only illustrative of the embodiments of the present disclosure and the technical principles employed. Those skilled in the art will appreciate that the present disclosure is not limited to the specific embodiments illustrated herein and that various obvious changes, adaptations, and substitutions are possible, without departing from the scope of the present disclosure. Therefore, although the present disclosure has been described in greater detail with reference to the above embodiments, the present disclosure is not limited to the above embodiments, and may include other equivalent embodiments without departing from the spirit of the present disclosure, the scope of which is determined by the scope of the appended claims.

Claims (28)

1. An error detection method, comprising:

obtaining a target pose of the tail end of the execution arm;

acquiring a positioning image;

identifying, in the positioning image, a plurality of markers located on the tip end of the effector arm, the plurality of markers including a plurality of pose markers for identifying a pose and at least one composite marker for identifying a pose and an angle;

determining an actual pose of the tip of the effector arm based on the at least one composite signature and the plurality of pose signatures; and

generating a fault-related control signal in response to the target pose and the actual pose satisfying an error detection condition.

2. The method of claim 1, wherein obtaining the target pose of the tip of the effector arm comprises:

determining the current pose of the main operator; and

and determining the target pose of the tail end of the execution arm based on the current pose of the main operator and the pose relation between the main operator and the tail end of the execution arm.

3. The method according to claim 2, characterized in that the pose relationships comprise at least one of:

the amount of change in the position of the tip of the actuator arm in the reference coordinate system is proportional to the amount of change in the position of the main operator in the reference coordinate system; or

The attitude variation of the tail end of the execution arm in the reference coordinate system is consistent with the attitude variation of the main manipulator in the reference coordinate system; or alternatively

The attitude of the tip of the execution arm in the reference coordinate system coincides with the attitude of the main manipulator in the reference coordinate system.

4. The method of claim 2, further comprising:

determining a previous pose of the primary operator;

determining a starting pose of the tip of the effector arm; and

determining the target pose based on a previous pose and a current pose of the master operator and a starting pose of an end of the execution arm.

5. The method of claim 2, further comprising:

determining a current position of a handle of the main operator relative to a base coordinate system of the main operator;

determining a previous position of the handle relative to a main operator base coordinate system;

determining a starting position of the end of the executing arm relative to an executing arm base coordinate system; and

and determining the target position of the tail end of the execution arm relative to the execution arm base coordinate system based on the previous position and the current position of the handle relative to the main operator base coordinate system, the transformation relation between the execution arm base coordinate system and the main operator base coordinate system and the starting position of the tail end of the execution arm relative to the execution arm base coordinate system.

6. The method of claim 2, further comprising:

determining the current posture of the handle of the main operator relative to the base coordinate system of the main operator;

determining a previous pose of the handle relative to the main operator base coordinate system;

determining a starting pose of the tip of the implement arm relative to the implement arm base coordinate system; and

and determining the target posture of the tail end of the execution arm relative to the execution arm base coordinate system based on the previous posture and the current posture of the handle relative to the main operator base coordinate system, the transformation relation of the execution arm base coordinate system and the main operator base coordinate system and the starting posture of the tail end of the execution arm relative to the execution arm base coordinate system.

7. The method of claim 5 or 6, further comprising:

and determining the transformation relation between the execution arm base coordinate system and the main operator base coordinate system based on the transformation relation between the execution arm base coordinate system and the camera coordinate system, the transformation relation between the camera coordinate system and the display coordinate system and the transformation relation between the display coordinate system and the main operator base coordinate system.

8. The method of claim 7,

the actuator arm base coordinate system has a predetermined transformation relationship with the camera coordinate system.

9. The method of claim 2, further comprising:

receiving current joint information of at least one joint of the master operator; and

determining a current pose of the master operator based on current joint information of at least one joint of the master operator.

10. The method of claim 5, further comprising:

receiving previous joint information of at least one joint of the master operator;

determining a previous pose of the primary manipulator based on previous joint information for at least one joint of the primary manipulator; and

and receiving the actual pose of the tail end of the executing arm obtained in the last round of detection cycle as the starting pose of the tail end of the executing arm.

11. The method of claim 1, further comprising:

determining three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in an end coordinate system of the effector arm based on the at least one composite marker; and

and determining the pose of the tail end coordinate system of the execution arm relative to a reference coordinate system as the actual pose based on the two-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the positioning image and the three-dimensional coordinates in the tail end coordinate system of the execution arm.

12. The method of claim 1, further comprising:

determining three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in a marker coordinate system;

determining a roll angle of the marker coordinate system relative to an end coordinate system of the effector arm based on the at least one composite marker;

determining three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the tip coordinate system of the effector arm based on a roll angle of the marker coordinate system relative to the tip coordinate system of the effector arm and the three-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the marker coordinate system; and

determining the pose of the end coordinate system of the executing arm relative to the reference coordinate system as the actual pose based on the two-dimensional coordinates of the at least one composite marker and the plurality of pose markers in the positioning image and the three-dimensional coordinates in the end coordinate system of the executing arm.

13. The method of claim 1, further comprising:

determining a plurality of candidate identifiers from the positioning image;

identifying a first token of the plurality of tokens from the plurality of candidate tokens; and

and searching other identifications by taking the first identification as a starting point.

14. The method of claim 13, further comprising:

in response to identifying the composite marker, identifying other markers based on a pose marker pattern matching template.

15. The method of claim 13, wherein the marker comprises a marker pattern and marker pattern corner points in the marker pattern, the method further comprising:

determining a region of interest in the positioning image;

dividing the region of interest into a plurality of sub-regions;

determining the pixel with the maximum corner likelihood value in each sub-region to form a pixel set;

determining a pixel with the maximum corner likelihood value in the candidate identifications as a candidate identification pattern corner; and

and matching the identification pattern matching template with the identification patterns at the corner positions of the candidate identification patterns to identify the first identification.

16. The method of claim 15, further comprising:

and in response to the failure of matching, determining the pixel with the maximum corner likelihood value in the rest pixels in the pixel set as a candidate identification pattern corner.

17. The method of claim 13, further comprising:

searching for a second identifier with the first identifier as a starting point;

determining a search direction based on the first identifier and the second identifier; and

and searching for the identifier in the searching direction by taking the first position identifier or the second identifier as a starting point.

18. The method of claim 17, further comprising:

in response to the fact that the search distance is larger than the search distance threshold value, determining a pixel with the largest corner likelihood value of the rest pixels in the pixel set as a candidate identification pattern corner; and

and matching the identification pattern matching template with the identification patterns at the positions of the corner points of the candidate identification patterns to identify the first identification.

19. The method of claim 17, further comprising:

in response to the number of identified markers being greater than or equal to a marker number threshold, determining the actual pose based on the identified markers.

20. The method of claim 17, further comprising:

in response to the fact that the number of the recognized identifiers is smaller than the identifier number threshold value, determining the pixel with the largest corner likelihood value of the rest pixels in the pixel set as a candidate identifier pattern corner; and

and matching the identification pattern matching template with the identification patterns at the positions of the corner points of the candidate identification patterns to identify the first identification.

21. The method of any of claims 1, 11-20, wherein a positioning tag is disposed on an outer surface of the post portion of the tip of the effector arm, the positioning tag comprising a plurality of marker patterns, the plurality of marker patterns comprising a plurality of different composite marker patterns and a plurality of pose marker patterns, the plurality of different composite marker patterns and the plurality of pose marker patterns being located in a same pattern distribution zone.

22. The method of claim 21, wherein at least one of N consecutive ones of the plurality of marker patterns comprises a composite marker pattern, wherein the composite marker pattern is different from the pose marker pattern and 2 ≦ N ≦ 4.

23. The method of claim 1, wherein the fault-related control signal comprises a first alarm signal indicating a fault in control of the implement arm.

24. The method of claim 1, further comprising:

receiving state information of at least one driving device for driving the execution arm in response to the target pose and the actual pose satisfying an error detection condition; and

and sending a second alarm signal in response to the state information and the driving information of the at least one driving device meeting a fault detection condition, wherein the second alarm signal indicates that the driving device of the execution arm is in fault.

25. The method of any one of claims 1-6, 8-20, 22-24, further comprising: and determining the target pose and the actual pose of the tail end of the executing arm at a preset period so as to carry out error detection on the executing arm in real time through a plurality of detection cycles.

26. A computer device, comprising:

a memory to store at least one instruction; and

a processor coupled with the memory and configured to execute the at least one instruction to perform the error detection method of any of claims 1-25.

27. A computer-readable storage medium storing at least one instruction that, when executed by a computer, causes the computer to perform the error detection method of any of claims 1-25.

28. A robotic system, comprising:

a master operator comprising a robot arm, a handle disposed on the robot arm, and at least one master operator sensor disposed at least one joint on the robot arm, the at least one master operator sensor for obtaining joint information of the at least one joint;

the tail end of the execution arm is provided with a plurality of marks, and the marks comprise a plurality of pose marks and at least one composite mark;

at least one drive device for driving the actuating arm;

at least one drive sensor coupled to the at least one drive and configured to obtain status information of the at least one drive;

the image acquisition equipment is used for acquiring a positioning image of the execution arm; and

a control device configured to be connected with the main operator, the at least one driving device sensor, the image capturing apparatus, to perform the error detection method according to any one of claims 1 to 25.

Images