CN114536401B - Robot system fault detection processing method based on multiple pose identifiers and robot system - Google Patents
Robot system fault detection processing method based on multiple pose identifiers and robot system Download PDFInfo
- Publication number
- CN114536401B CN114536401B CN202210141440.3A CN202210141440A CN114536401B CN 114536401 B CN114536401 B CN 114536401B CN 202210141440 A CN202210141440 A CN 202210141440A CN 114536401 B CN114536401 B CN 114536401B
- Authority
- CN
- China
- Prior art keywords
- pose
- identifiers
- determining
- arm
- corner
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 45
- 238000003672 processing method Methods 0.000 title claims abstract description 12
- 238000000034 method Methods 0.000 claims abstract description 155
- 238000004891 communication Methods 0.000 claims description 74
- 230000004044 response Effects 0.000 claims description 34
- 238000009826 distribution Methods 0.000 claims description 26
- 230000015654 memory Effects 0.000 claims description 21
- 238000003860 storage Methods 0.000 claims description 20
- 230000033001 locomotion Effects 0.000 claims description 19
- 238000011084 recovery Methods 0.000 claims description 13
- 238000007476 Maximum Likelihood Methods 0.000 claims description 11
- 230000004807 localization Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 abstract description 22
- 230000000875 corresponding effect Effects 0.000 description 22
- 238000004422 calculation algorithm Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 12
- 230000006870 function Effects 0.000 description 10
- 230000008569 process Effects 0.000 description 9
- 238000003384 imaging method Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000013507 mapping Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 208000032368 Device malfunction Diseases 0.000 description 2
- 238000002591 computed tomography Methods 0.000 description 2
- 238000003745 diagnosis Methods 0.000 description 2
- 239000012636 effector Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 239000003550 marker Substances 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000000926 separation method Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000005520 cutting process Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/02—Sensing devices
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
The disclosure relates to the technical field of fault detection processing, and discloses a robot system fault detection processing method. The robot system comprises a control device, at least one driving device and an executing arm, wherein the at least one driving device drives the executing arm based on a driving signal sent by the control device. The method comprises the following steps: determining a target pose of the tail end of the execution arm; acquiring a positioning image; in the positioning image, identifying a plurality of pose identifiers positioned on the end of the execution arm, wherein the plurality of pose identifiers comprise different pose identifier patterns; determining an actual pose of the end of the execution arm based on the plurality of pose identifiers; enabling the robot system to enter a safety mode based on the fact that the target pose and the actual pose meet error conditions; and determining a fault condition of the robotic system.
Description
Technical Field
The disclosure relates to the technical field of fault detection processing, in particular to a robot system fault detection processing method and a robot system based on multiple pose identifications.
Background
During the operation of the robot system, the working condition of the robot system needs to be detected, and necessary countermeasures are taken when faults occur, otherwise, the operation may fail, and even irrecoverable losses are caused.
The execution arm is an important component of the robot system, and the pose accuracy of the execution arm can be used as an important index for measuring whether the robot system has faults or not. In the prior art, equipment such as a laser tracker can be used for measuring the pose of an execution arm, however, the equipment such as the laser tracker at least has the technical problems of high price, incapability of measuring the pose at one time, need of regular calibration and the like.
On the other hand, when the existing robot system detects that a fault occurs, the whole robot system is stopped and detected. In the actual production operation, the processing mode can reduce the operation efficiency and influence the user experience.
Therefore, it is necessary to provide a method capable of simply and effectively detecting the failure of the robot system, and adopting corresponding processing measures according to the failure condition, so as to consider both the safety and the user experience of the robot system.
Disclosure of Invention
In some embodiments, the present disclosure provides a robot system fault detection processing method. The robot system comprises a control device, at least one driving device and an executing arm, wherein the at least one driving device drives the executing arm based on a driving signal sent by the control device. The method may include: determining a target pose of the tail end of the execution arm; acquiring a positioning image; in the positioning image, identifying a plurality of pose identifiers positioned on the end of the execution arm, wherein the plurality of pose identifiers comprise different pose identifier patterns; determining an actual pose of the end of the execution arm based on the plurality of pose identifiers; enabling the robot system to enter a safety mode based on the fact that the target pose and the actual pose meet error conditions; and determining a fault condition of the robotic system.
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 the 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, cause the computer to perform a method of any one of some embodiments of the present disclosure.
In some embodiments, the present disclosure provides a robotic system comprising: the tail end of the execution arm is provided with a plurality of pose marks, and the pose marks comprise different pose mark patterns; at least one driving device for driving the actuator 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 device is used for acquiring a positioning image of the execution arm; and a control device configured to interface with the at least one drive device, the at least one drive device sensor, the image acquisition apparatus, perform the method of any of the 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 a fault detection processing system according to some embodiments of the present disclosure;
FIG. 3 illustrates a schematic diagram of a tag including multiple pose identifications according to some embodiments of the present disclosure;
FIG. 4 illustrates a schematic view of a label disposed on a peripheral side of an end of an actuator arm and formed in a cylindrical shape according to some embodiments of the present disclosure;
FIG. 5 illustrates a flow chart of a fault detection processing method according to some embodiments of the present disclosure;
FIG. 6 illustrates a flow chart of a method of fault handling based on a fault condition of a robotic system according to some embodiments of the present disclosure;
fig. 7 illustrates a flow chart of a method for a communication failure detection process of a robotic system according to some embodiments of the present disclosure;
FIG. 8 illustrates a flowchart of a method for an arm fault detection process of a robotic system according to some embodiments of the present disclosure;
FIG. 9 illustrates a flowchart of a method for a target pose fault detection process for a robotic system according to some embodiments of the present disclosure;
FIG. 10 illustrates a flowchart of a method of determining three-dimensional coordinates of a plurality of pose identifiers relative to an end coordinate system of an implement arm, according to some embodiments of the present disclosure;
FIG. 11 illustrates a flowchart of a method of determining three-dimensional coordinates of a plurality of pose identifiers relative to an end coordinate system of an implement arm according to further embodiments of the present disclosure;
FIG. 12 illustrates a flowchart of a method of identifying pose identifiers according to some embodiments of the present disclosure;
FIG. 13 illustrates a schematic diagram of a pose identification pattern according to some embodiments of the present disclosure;
FIG. 14 illustrates a flowchart of a method for searching for pose identifications according to some embodiments of the present disclosure;
FIG. 15 illustrates a schematic diagram of search gesture identification in accordance with some embodiments of the present disclosure;
FIG. 16 illustrates a flowchart of a method for searching for a second pose identification according to some embodiments of the present disclosure;
FIG. 17 illustrates a flowchart of a method for searching for pose identifications according to some embodiments of the present disclosure;
FIG. 18 illustrates a schematic block diagram of a computer device in accordance with some embodiments of the present disclosure;
fig. 19 illustrates a schematic diagram of a robotic system according to some embodiments of the present disclosure.
Detailed Description
In order to make the technical problems solved by the present disclosure, the technical solutions adopted and the technical effects achieved clearer, the technical solutions of the embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. It will be appreciated by those skilled in the art that the described embodiments are to be considered in all respects as illustrative and not restrictive, and that only illustrative embodiments of the disclosure, and not all embodiments.
In this disclosure, the term "position" refers to the positioning of an object or a portion of an object in three dimensions (e.g., three translational degrees of freedom may be described using Cartesian X, Y and changes in Z coordinates, such as along the 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, which may be described using roll, pitch, and yaw). In the present disclosure, the term "pose" refers to a combination of position and pose of an object or 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 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 select 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 camera coordinate system or the operator's own perception coordinate system, or the like. The camera coordinate system refers to a coordinate system where the image acquisition equipment is located.
In this disclosure, an object may be understood as an object or target that needs to be positioned, such as an actuator arm or an end of an actuator arm. The pose of the actuator arm or a part thereof may refer to the pose of the coordinate system defined by the actuator arm or a part thereof with respect 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 connected to the master carriage 110 and the slave carriage 130, for example, by a cable connection or by a wireless connection, so as to communicate with the master carriage 110 and the slave carriage 130. The master cart 110 includes a master manipulator for teleoperation by an operator, a display for displaying an image of an operation area, and a touch screen for touch operation by the operator, and the slave cart 130 includes an execution arm for executing a job. The master-slave mapping between the master manipulator in the master carriage 110 and the execution arm in the slave carriage 130 is realized by the control device 120, and the motion control of the master manipulator to the execution arm is realized. In some embodiments, the actuator arm is configured to be able to enter the operating region through a sheath, or the like, and to contact and generate a force at the operating region with an object to be operated (e.g., a workpiece, human tissue, or the like). The sleeve and the sheath can be fixed on the wall surface of the wall body, the animal body and the like at an opening (such as a manual opening or a natural opening) and the operation area can be an area where the operation is performed. The implement arm may be a deformable arm and a distal end of the implement arm may be provided with a distal instrument (e.g., an effector) that may include, but is not limited to, an excavating instrument, an underwater working 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.
Fig. 2 illustrates a schematic diagram of a fault detection processing system 200 (hereinafter also referred to as "system 200") according to some embodiments of the present disclosure. As shown in fig. 2, the system 200 may include a control device 220, an actuator arm 230, at least one drive device 260, and an image capture device 250. The actuator arm 230 may be implemented as a deformable arm or a rigid arm. In some embodiments, the actuator arm 230 may include an actuator arm tip 231 at the tip or distal end. In some embodiments, a distal instrument 240 may be provided at the distal end of the implement arm tip 231.
In some embodiments, the control device 220 may be communicatively coupled to at least one drive device 260 and send drive signals to the drive device 260 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 driving device 260 may be a servo motor, and may receive an instruction of the control device to control the movement of the actuator arm 230, so that the distal end 231 of the actuator arm moves to the corresponding target pose. The control device 220 may be communicatively coupled to a drive device sensor coupled to the drive device 260, for example, via a communication interface, to receive operational data of the drive device 260 to enable status monitoring of the drive device 260. In one example of the present disclosure, the communication interface may be a CAN (ControllerAreaNetwork) bus communication interface that enables the control device 220 to communicate with the drive device 260 and drive device sensor connections via a CAN bus.
In some embodiments, as shown in fig. 2, the system 200 may further include at least one brake unit 270 for stopping the actuation of the actuator arm 230, the at least one brake unit 270 may be communicatively coupled to the control device 220. In some embodiments, at least one brake unit 270 may be provided corresponding to at least one driving device 260, and movement of the driving device 260 is stopped based on a brake signal from the control device 220. For example, brake unit 270 may stop movement of drive 260 via mechanical braking (e.g., electromagnetic band-type brake) or electrical braking based on the brake signal. In some embodiments, the at least one brake unit 270 may also be used to break the drive connection between the at least one drive 260 and the corresponding drive mechanism, thereby stopping actuation of the actuator arm 230.
The control means 220 may be communicatively connected to an image acquisition device 250. In some embodiments, the image acquisition device 250 may be used to acquire a positioning image. The positioning image may include a partial or complete image of the actuator arm 230. In some embodiments, 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. In some embodiments, the image acquisition device 250 may be used to acquire images of the distal end 231 of the actuator arm, and the distal end 231 of the actuator arm may have a plurality of different pose identifiers disposed thereon, including different pose identifier patterns. For example, the actuator arm end 231 may be provided with a positioning tag 232 (the positioning tag 232 may be, for example, the tag 300 shown in fig. 3). The positioning tag 232 may include a plurality of pose identifiers including different pose identifier patterns (as described in more detail below).
As shown in fig. 2, the distal end 231 of the actuator arm is within the field of view 251 of the image acquisition device 250, and an image of the distal end 231 of the actuator arm may be included in the acquired positioning image. Depending on the application scenario, the image capture device 250 may be an industrial camera, an underwater camera, a miniature electronic camera, an endoscopic camera, etc. In some embodiments, the image acquisition device 250 may be fixed in position or variable in position, for example, an industrial camera fixed in a monitored location or an endoscopic camera adjustable in position or pose. In some embodiments, the image acquisition device 250 may implement at least one of visible light band imaging, infrared band imaging, CT (Computed Tomography, electronic computed tomography) imaging, acoustic wave imaging, and the like. Depending on the kind of the acquired image, a person skilled in the art may select a different image acquisition apparatus as the image acquisition apparatus 250.
In some embodiments, the control 220 may receive the positioning image from the image acquisition device 250 and process the positioning image. For example, the control device 220 may identify a plurality of pose identifiers and at least one angle identifier located on the execution arm tip 231 in the positioning image, and determine the pose of the execution arm tip 231 with respect to a reference coordinate system (e.g., a world coordinate system) as an actual pose of the execution arm tip 231.
In some embodiments, the system 200 may determine whether the robotic system is malfunctioning based on the actual pose of the implement arm tip 231 and the target pose. In some embodiments, the control 220 may cause the robotic system to enter a safe mode and determine a fault condition of the robotic system upon detecting the fault. In some embodiments, the control device 220 may also determine the actual pose of the implement arm tip 231 at predetermined periods to determine the fault condition of the robotic system in real time through a plurality of fault detection cycles. In some embodiments, the control device 220 may generate a fault alert signal corresponding to a fault condition based on the fault condition of the robotic system. In some embodiments, the fault alert signal may be represented by a sound, a light, a prompt displayed on a screen (e.g., touch screen), a combination thereof, or the like. In some embodiments, the fault alert signal may include a plurality of different types of alert signals according to the type of fault, which may be represented by different sounds, different colored lights, or different prompts. In some embodiments, the fault alert signal may include a prompt asking the user if the manual mode is used. For example, the fault alert signal may include a prompt message and a manual mode selection menu. Those skilled in the art will appreciate that system 200 may be applied to dedicated or general purpose robotic systems in a variety of fields (e.g., logistics, industrial manufacturing, medical, etc.), either teleoperational or non-teleoperational. As one example, the system 200 may be applied to a robotic system, such as a surgical robot, and the end instrument 240 disposed at the distal end of the implement arm end 231 may be a surgical effector, for example.
In some embodiments, a plurality of pose identifiers are distributed on the actuator arm (e.g., on the actuator arm tip 231). In some embodiments, a plurality of pose markers are disposed on an outer surface of the columnar portion of the actuator arm 230. For example, the plurality of pose markers are circumferentially distributed on the actuator arm tip 231, e.g., circumferentially disposed on an outer surface of the cylindrical portion of the actuator arm tip 231. In some embodiments, a positioning tag 232 including a plurality of pose identifiers including a plurality of different pose identifier patterns distributed on the positioning tag along the circumferential direction of the columnar portion and pose identifier pattern corner points among the pose identifier patterns is provided on the outer surface of the columnar portion of the arm tip 231.
In some embodiments, the pose identification may include a pose identification pattern and pose identification pattern corner points in the pose identification pattern. In some embodiments, the pose 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 end of the actuator arm itself, for example, may include depressions or protrusions, and combinations thereof. In some embodiments, the pose identification pattern may include a pattern formed in brightness, gray scale, color, and the like. In some embodiments, the pose identification pattern may include a pattern that provides information detected by the image acquisition device, either actively (e.g., self-light emitting) or passively (e.g., reflected light). Those skilled in the art will appreciate that in some embodiments, the pose of the pose identification or pose of the pose identification pattern may be represented by the pose of the pose identification pattern corner coordinate system. In some embodiments, the pose identification pattern is provided on the end of the actuator arm in an area adapted to be imaged by the image acquisition device, e.g. 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 blocked during operation.
FIG. 3 illustrates a schematic diagram of a tag 300 including multiple pose identifications, according to some embodiments. Fig. 4 shows a schematic view of a label 400 provided on the peripheral side of the distal end of the actuator arm and formed in a cylindrical shape. It will be appreciated that for simplicity, the tag 300 may include the same pose identification pattern as the tag 400.
Referring to fig. 3, the plurality of pose identifiers may include a plurality of different pose identifier patterns 310. The plurality of pose identifications may also include a plurality of pose identification pattern corner points P in a plurality of different pose identification patterns 310 3 The pose identification pattern corner is denoted by a "good" symbol in this disclosure. In some embodiments, the bit may be identified by an identification bitGesture identification pattern 310 or gesture identification pattern corner P therein 3 And determining pose identification.
Referring to fig. 4, in the circumferentially disposed state, the label 300 becomes a label 400 spatially configured in a cylindrical shape. In some embodiments, the pivot angle or roll angle of the pose identification may be represented by the pivot angle of the pose identification pattern or the pose identification pattern corner. The pivot angle of each pose identification pattern or pose identification pattern corner identification is known or predetermined. In some embodiments, based on a distribution of a plurality of pose identifications (e.g., pose identification patterns or pose identification pattern corner points), an axis-around angle identified by each pose identification may be determined. In some embodiments, the plurality of pose identifiers may be evenly distributed (e.g., the pose identifier pattern corner in tag 300 is equally spaced apart and the pose identifier pattern corner in tag 400 is equally distributed). In other embodiments, the plurality of pose identifiers may be unevenly distributed. In some embodiments, each pose identification pattern may be used to identify a particular pivot angle based on a distribution of multiple pose identifications, each pose identification pattern having a one-to-one correspondence with the identified pivot angle. In this disclosure, the pivot angle or roll angle refers to an angle about a Z-axis (e.g., the Z-axis of the end coordinate system { wm } of the actuator arm). In some embodiments, the Z-axis may be tangential to the end of the actuator arm.
As shown in fig. 4, a plurality of different pose identification patterns 410 in the tag 400 are uniformly distributed along the circumferential direction of the cylindrical structure, and a plurality of pose identification pattern corner points are uniformly distributed on a cross-sectional circle 420 of an XY plane of an end coordinate system { wm } of the actuator arm, then the distribution angle (e.g., angle α 0 ) Equal. Setting a pose identification pattern corner point P pointed by X axis 4 ,P 4 As a reference corner (pose identification pattern corner P) for identifying 0 degree pivot angle 4 The located pose identification pattern is used as a reference pattern), the corner points of the arbitrary pose identification pattern and the corner point P of the pose identification pattern can be obtained according to the arbitrary pose identification pattern 4 And determining the pivot angle of the pose identification pattern corner mark. In some embodiments, the wrapping of pose identification pattern corner identificationsThe shaft angle may be determined based on the following formula (1):
α m =α 0 (m-1) (1)
wherein alpha is m To identify pattern corner P with pose 4 As the first pose identification pattern corner, the mth pose identification pattern corner has an around-axis angle according to the clockwise direction of the cross-section circle 420.
Some embodiments of the present disclosure provide a fault detection processing method for a robotic system. Fig. 5 illustrates a flow chart of a fault detection processing method 500 (hereinafter also referred to simply as "method 500") according to some embodiments of the present disclosure. Method 500 may be implemented or performed by hardware, software, or firmware. In some embodiments, the method 500 may be performed by a robotic system (e.g., the robotic system 100 shown in fig. 1, or the robotic system 1900 shown in fig. 19). In some embodiments, method 500 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 500. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to fig. 5, at step 501, a target pose of the end of the actuator arm is determined. In some embodiments, the target pose of the tip of the actuator arm may be preset. For example, the target pose of the tip of the actuator arm may be input in advance by the user through an input device and stored in a memory of the robot system. Alternatively, the target pose of the tip of the execution arm may also be obtained by the pose generation unit through random generation. In some embodiments, the target pose of the tip of the actuator arm may be determined based on the pose of the main manipulator. For example, during teleoperation, a target pose of the distal end of the actuator arm may be determined from the pose of the main operator based on a master-slave motion mapping relationship between the pose of the main operator and the pose of the distal end of the actuator arm.
In some embodiments, the control device 220 may send a drive signal to the at least one drive device 260 to control the movement of the distal end 231 of the actuator arm based on the target pose of the distal end of the actuator arm. In some embodiments, the control device 220 may determine the drive signal of the at least one drive device controlling the motion of the actuator arm by performing an inverse kinematics numerical iterative algorithm of the arm kinematics model based on the target pose of the end of the actuator arm. It should be appreciated that the kinematic model may represent a mathematical model of the kinematic relationship of the joint space and task space of the execution arm. For example, the kinematic model may be established by DH (Denavit-Hartenberg) parametric method, exponential product representation method, and the like. In some embodiments, the target pose of the tip of the actuator arm is a target pose of the tip of the actuator arm in a reference coordinate system.
With continued reference to fig. 5, in step 503, a positioning image is acquired. In some embodiments, the positioning image includes a plurality of pose identifications on the distal end of the actuator arm. In some embodiments, the positioning image may be received from an image acquisition device 250 as shown in fig. 2. For example, the control 220 may receive a positioning image actively transmitted by the image acquisition device 250. Alternatively, the control device 220 may transmit an image request instruction to the image pickup apparatus 250, and the image pickup apparatus 250 transmits a positioning image to the control device 220 in response to the image request instruction.
With continued reference to FIG. 5, in step 505, a plurality of pose identifiers located on the distal end of the execution arm are identified in the positioning image, the plurality of pose identifiers including different pose identifier patterns. For example, exemplary methods of identifying a plurality of pose identifiers located on the distal end of an execution arm may include the methods shown in fig. 12, 14, 16, and 17. In some embodiments, the control device 220 may identify a partial or complete pose identification in the localization image through an image processing algorithm. In some embodiments, the image processing algorithm may include a feature recognition algorithm, which may extract or recognize features of the pose identification. For example, the image processing algorithm may comprise a corner detection algorithm for detecting pose identification pattern corners. The corner detection algorithm may be one of, but not limited to, gray-graph based corner detection, binary image based corner detection, contour curve based corner detection. For example, the image processing algorithm may be a color feature extraction algorithm for detecting color features in the pose identification pattern. For another example, the image processing algorithm may be a contour detection algorithm for detecting contour features of the pose identification pattern. In some embodiments, the control device may identify the pose identification of some or all of the positioning images by identifying the model.
With continued reference to FIG. 5, at step 507, an actual pose of the end of the actuator arm is determined based on the plurality of pose identifiers. In some embodiments, the method 500 further comprises: determining two-dimensional coordinates of the plurality of pose identifiers in the positioning image; and determining the pose of the end coordinate system of the execution arm relative to the reference coordinate system as the actual pose of the end of the execution arm based on the two-dimensional coordinates of the plurality of pose identifications in the positioning image and the three-dimensional coordinates of the plurality of pose identifications relative to the end coordinate system of the execution arm. In some embodiments, the coordinates of the pose identification may be represented by the coordinates of the pose identification pattern corner points. For example, the two-dimensional coordinates of the pose identification in the positioning image and the three-dimensional coordinates in the end coordinate system of the execution arm may be represented by coordinates of the pose identification pattern corner points. In some embodiments, the pose of the end coordinate system of the actuator arm relative to the reference coordinate system may be determined as the actual pose of the end of the actuator arm based on the two-dimensional coordinates of the plurality of pose identification pattern corner points in the positioning image and the three-dimensional coordinates in the end coordinate system of the actuator arm.
With continued reference to fig. 5, at step 509, the robotic system is brought into a safe mode based on the target pose satisfying an error condition with the actual pose. For example, the control device may determine a target pose of the end of the execution arm in the reference coordinate system, determine an actual pose of the end of the execution arm based on the positioning image of the execution arm, and determine that the robot system fails when the target pose and the actual pose satisfy an error condition (e.g., are greater than or equal to an error threshold), so as to enable the robot system to enter a safety mode. In the present disclosure, the safety mode may be a mode in which, when a failure of the robot system is detected, an action of the execution arm related to the failure is stopped to ensure the safety of the robot work.
In some embodiments, causing the robotic system to enter the safe mode may include disabling at least one drive device that drives the actuator arm. For example, when the system 200 determines that the robotic system is malfunctioning and enters a safe mode, the control device 220 may send a braking signal to at least one braking unit 270 corresponding to at least one drive device 260, the at least one braking unit 270 braking the at least one drive device 260 based on the braking signal to stop the actuation of the actuator arm 230 and maintain the actuator arm tip 231 in a pose. In some embodiments, placing the robotic system into a safe mode may also include allowing a user of the robotic system to use a manual mode in which the user may participate in deciding how to handle the failure of the robotic system, as described in detail below.
In some embodiments, the method 500 may further include determining a pose difference based on a target pose of the tip of the implement arm in the reference frame and an actual pose of the tip of the implement arm in the reference frame, and determining whether the robot is malfunctioning based on the pose difference and the error condition. The pose difference may include a position difference and a pose difference. In some embodiments, the pose difference between the target pose and the actual pose of the tail end of the execution arm can be determined through one-time execution arm detection, and when the pose difference meets the error condition, the robot system is determined to be faulty, so that the robot system enters a safety mode. In some embodiments, multiple pose differences between the target pose and the actual pose of the end of the execution arm may be determined by performing the arm detection multiple times, and when the pose differences meet an error condition (e.g., an average or cumulative value of the multiple pose differences is greater than or equal to an error threshold), a failure of the robotic system may be determined, thereby enabling the robotic system to enter a safe mode. In some embodiments, the method 500 may further include determining the actual pose of the end of the implement arm at a predetermined detection period (e.g., steps 503-507 are performed at a predetermined detection period) to determine or cumulatively determine whether the robotic system is malfunctioning over a plurality of detection cycles in real-time.
In the kth execution arm detection cycle, the pose difference may be expressed as follows:
wherein,for the difference in position of the actuator arm at the kth actuator arm detection cycle, +.>For the angle difference of the actuator arm at the kth actuator arm detection cycle, P t k For the target position of the actuator arm at the kth actuator arm detection cycle, R t k For the target pose of the actuator arm at the kth actuator arm detection cycle, < >>For the actual position of the actuator arm at the kth actuator arm detection cycle, R r k For the actual pose of the actuator arm at the kth actuator arm detection cycle, < >>Representation->And->And a corner therebetween.
With continued reference to fig. 5, at step 511, a fault condition of the robotic system is determined. In the safe mode, the system 200 may perform a fault diagnosis of the robotic system, determining a fault condition of the robotic system. In some embodiments, diagnosing the robotic system for a fault may include the control device detecting the actuator arm and its associated components (e.g., drive device, communication module, etc.) to determine the location, status, etc. of the fault when the robotic system enters a safe mode. In the present disclosure, the fault condition of the robot may include a recoverable fault and an unrecoverable fault. In some embodiments, a recoverable fault may, for example, attempt to recover from the fault, and after the fault recovery is successful, the robotic system may be brought online again to continue operation. An unrecoverable failure, for example, represents an inability of the robotic system to continue operation, requiring suspension of system operation and corresponding action.
In some embodiments, the method 500 may further include performing fault handling corresponding to the fault condition based on the fault condition of the robotic system. Fig. 6 illustrates a flow chart of a method 600 of fault handling based on a fault condition of a robotic system according to some embodiments of the present disclosure. Method 600 may be implemented or performed by hardware, software, or firmware. In some embodiments, the method 600 may be performed by a robotic system (e.g., the robotic system 100 shown in fig. 1, or the robotic system 1900 shown in fig. 19). In some embodiments, method 600 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 600. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to FIG. 6, in step 601, a fault condition is determined. In some embodiments, when the robotic system enters a safe mode, the control device 220 may simultaneously perform fault diagnostics on multiple components on the actuator arm associated with the fault to determine the specific location and cause of the fault. Alternatively, when the robot system enters the safety mode, the control device 220 may sequentially perform fault diagnosis on a plurality of components on the execution arm related to the fault, and remove components that have not failed one by one, so as to determine the fault condition of the robot system. Step 601 may be implemented similarly to step 511 in method 500. The fault conditions may include faults of different fault types, such as recoverable faults and unrecoverable faults.
With continued reference to FIG. 6, in step 603, a determination is made as to whether the fault condition is a recoverable fault. In some embodiments, the failure of the robotic system may be pre-classified as either a recoverable failure or a non-recoverable failure, and the classification stored as a failure detection table in the memory of the robotic system, upon detection of the failure, compared to the failure detection table to determine if the failure condition is a recoverable failure.
With continued reference to fig. 6, in response to determining that the fault condition of the robotic system is a recoverable fault, proceeding to step 605, a fault recovery operation is performed. The failback operation may include, for example, a system restart, a module restart, a driver initialization, etc., based on the failure condition. In some embodiments, the control device 220 may issue a fault alert signal to indicate a fault is occurring and/or automatically perform a fault recovery operation to attempt to recover from the fault upon determining that the fault condition is a recoverable fault. In other embodiments, the control device 220 may also issue a fault alert signal (e.g., a prompt message and menu displayed on a touch screen) to ask the user if a fault recovery operation is performed. In some embodiments, the control device 220 may automatically perform the fault recovery operation when the fault alert signal passes a prescribed time but the user is not yet operating.
On the other hand, in response to determining that the fault condition of the robotic system is an unrecoverable fault, proceed to step 611 to cause the robotic system to enter an exit mode. In some embodiments, the control 220 may issue a fault alert signal (e.g., display a prompt and menu on a touch screen) to ask the user if to put the robotic system into an exit mode. In addition, when the failure condition of the robot system is a recoverable failure but the user is not allowed to perform a failure recovery operation, the robot system may be put into the exit mode. In the exit mode, the user may activate the automatic exit function of the robotic system, for example, via a touch screen, to safely withdraw the actuator arm 230 and its tip 231 from the operating area. Alternatively, the user may activate the manual exit function via the touch screen to manually exit the actuator arm 230 according to a predetermined operation flow.
With continued reference to FIG. 6, at step 607, a determination is made as to whether the failure recovery was successful. In some embodiments, control 220 may, after performing the fault recovery operation, again determine a fault condition for the component that failed to recover to determine if the fault was successfully recovered. Step 607 may be implemented similarly to step 511 in method 500 or step 601 in method 600.
With continued reference to fig. 6, in response to successful in the failback operation, step 609 is entered to cause the robotic system to exit the safe mode. Alternatively, in response to failure recovery operation failure, proceed to step 611 to cause the robotic system to enter an exit mode.
In some embodiments, the robotic system failure may include a communication failure.
In some embodiments, the robotic system may include a communication node for relaying the drive signal sent by the control device and a communication module for communicating with the communication node. The communication node may be communicatively connected to, for example, the control device 220 in fig. 2 via a communication module, and may transmit a drive signal from the control device 220 to the at least one drive device 260. In some embodiments, the communication node may comprise a control chip corresponding to the at least one driving device 260 for performing communication functions and appropriate processing functions. For example, the control chip may be used to calculate the pose of the tip 231 of the actuator arm 230 based on the state information of the drive device 260, or to calculate the drive signal of the drive device 260 based on the control signal from the control device 220.
Fig. 7 is a flow chart illustrating a method 700 for a communication failure detection process of a robotic system according to some embodiments of the present disclosure. Method 700 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 700 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). In some embodiments, method 700 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 700. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to fig. 7, an inquiry signal is sent to a communication node in step 701. In some embodiments, when the robotic system enters the safe mode, the control device 220 may send an interrogation signal to the communication node through the communication module to determine whether the communication of the robotic system is normal. For example, the control device 220 may send an inquiry signal to the communication node through the communication module, and the communication node may send a response signal to the control device 220 through the communication module when receiving the inquiry signal, and the control device 220 determines whether the robot system has a communication failure based on whether the response signal is received.
In step 703, a communication failure alarm signal indicating that a communication failure has occurred and/or the communication module is restarted in response to failure to receive the response signal of the communication node. For example, upon failure to receive a reply signal from a communication node, the control device 220 may generate a communication failure alarm signal to indicate that a communication failure has occurred, and/or automatically restart the communication module to attempt to recover from the communication failure. In some embodiments, the control device 220 may also generate a communication failure alarm signal (e.g., display a prompt and menu on a touch screen) to ask the user if to restart the communication module. Alternatively, the control device 220 may automatically restart the communication module when the user does not operate for a long time (e.g., more than a prescribed time) after the communication failure alarm signal is generated.
In some embodiments, the communication node may send signals to the control device 220 at regular time. The control means 220 may generate a communication failure alarm signal indicating occurrence of a communication failure and/or restart the communication module in response to not receiving a signal from the communication node for a predetermined time.
In other embodiments, the robotic system may also not comprise a communication node, but a communication module for communicatively connecting the control device with the at least one drive device. The control device 220 receives status information of the at least one drive device 260 (e.g., may be transmitted by a drive device sensor coupled to the at least one drive device 260) via the communication module to determine whether a communication failure exists in the robotic system. In some embodiments, in response to failure to receive status information of at least one drive device, a communication failure alarm signal is generated indicating that a communication failure occurred and/or a communication module for communicating with at least one drive device is restarted. For example, when status information of at least one of the driving apparatuses 260 cannot be received, the control apparatus 220 may generate a communication failure alarm signal to indicate that a communication failure occurs, and/or automatically restart the communication module to attempt to recover from the communication failure. In some embodiments, the control device 220 may also generate a communication failure alarm signal to ask the user whether to restart the communication module or automatically restart the communication module when the user does not operate for a long time.
In some embodiments, the robotic system is caused to exit the safe mode in response to a communication failure recovery after a restart. For example, after automatically or manually re-communicating the module by a user, the control device 220 determines that the robotic system has successfully recovered from the fault and exits the safe mode to continue performing operations if a response signal from the communication node or status information of the at least one driving device 260 can be received and no other obstacle is detected. In some embodiments, the robotic system is caused to enter an exit mode in response to a communication failure not recovering after a restart. For example, after the automatic or manual re-communication module, when the control device 220 still cannot receive the response signal from the communication node or the status information of the at least one driving device 260, it is determined that the robot system cannot continue to operate, so that the robot system enters the exit mode.
In some embodiments, the robotic system failure may include a drive failure or an implement arm failure.
In some embodiments, the control device sends a drive signal to the at least one drive device based on a target pose of the tip of the actuator arm and receives status information of the at least one drive device for driving the actuator arm via the drive device sensor. In some embodiments, the control device determines a fault condition of the robotic system based on the status information of the at least one drive device and the drive signal of the at least one drive device when the robotic system enters the safe mode.
In some embodiments, a drive failure alarm signal is generated in response to the status information and the drive signal of at least one drive device issued by the control device satisfying a drive device failure condition, the drive device failure signal indicating that the drive device of the actuator arm is failed. For example, in the safety mode, the control device 220 may determine that at least one driving device 260 is malfunctioning when the status information of the at least one driving device 260 and the driving signal transmitted thereto satisfy a driving device malfunction condition (e.g., greater than or equal to an error threshold), generate a driving device malfunction alarm signal and/or cause the robot system to enter an exit mode, or may query a user (e.g., display a prompt message and a menu on a touch screen) whether to cause the robot system to enter an exit mode.
In some embodiments, in response to the status information not satisfying the drive device fault condition with the drive signal of the at least one drive device issued by the control device, an actuator arm fault alert signal is generated and/or the robotic system is put into an exit mode, the actuator arm fault alert signal indicating the occurrence of an actuator arm fault. For example, in the safety mode, the control device 220 may determine that the at least one driving device 260 is operating normally and the execution arm 230 is malfunctioning when the status information of the at least one driving device 260 and the driving signal transmitted thereto do not satisfy the driving device fault condition (e.g., are less than the error threshold), generate an execution arm fault alarm signal and/or cause the robot system to enter the exit mode, or may query the user (e.g., display a prompt message and a menu on a touch screen) whether to cause the robot system to enter the exit mode.
Fig. 8 is a flowchart illustrating a method 800 for an arm fault detection process of a robotic system according to some embodiments of the present disclosure. Method 800 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 800 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). In some embodiments, method 800 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 800. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to fig. 8, at step 801, actual joint information for at least one joint of an actuator arm is received. In some embodiments, the actuator arm comprises at least one joint, at which at least one actuator arm sensor is provided for obtaining actual joint information of the at least one joint. The actual joint information may include an actual position or an actual angle of the joint.
In step 803, target joint information for at least one joint of the actuator arm is determined based on the target pose of the end of the actuator arm. It should be appreciated that in robotic systems, the pose of the actuator arm may be represented by a set of joint information for the joints of the actuator arm (e.g., a one-dimensional matrix made up of such joint information). In case the target pose of the end of the actuator arm is preset or randomly generated, the target joint information of at least one joint of the actuator arm may be determined, for example, by a inverse kinematics algorithm.
In step 805, in response to the actual joint information and the target joint information meeting the arm fault condition, an arm fault alert signal is generated and/or the robotic system is put into an exit mode, the arm fault alert signal indicating that an arm fault has occurred. For example, in the safety mode, the control device 220 may determine that the actuator arm 230 fails when the actual joint information and the target joint information of at least one joint satisfy the actuator arm failure condition (e.g., greater than or equal to the error threshold), generate an actuator arm failure alarm signal and/or cause the robot system to enter the exit mode, or may query the user (e.g., display a prompt message and a menu on a touch screen) whether to cause the robot system to enter the exit mode.
In some embodiments, the robotic system failure may include a target pose of the end of the implement arm exceeding a motion limit of the implement arm. Fig. 9 is a flow chart illustrating a method 900 for a target pose fault detection process for a robotic system according to some embodiments of the disclosure. Method 900 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 900 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). 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 (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 900. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to FIG. 9, at step 901, a determination is made as to whether the target pose exceeds the motion limits of the actuator arm. In some embodiments, the working space of the actuator arm may be pre-stored in a memory of the robotic system, the working space of the actuator arm comprising the motion limits of the actuator arm. In some embodiments, it may occur that the randomly generated target pose exceeds the motion limits of the actuator arm, thereby resulting in the inability of the end of the actuator arm to achieve the randomly generated target position and/or pose. In some embodiments, for example, during teleoperation, it may occur that the target pose of the end of the actuator arm corresponding to the pose of the main manipulator exceeds the motion limit of the actuator arm.
In step 903, in response to the target pose exceeding the motion limit of the actuator arm, an alarm signal is generated indicating that the motion limit is exceeded.
At step 905, the secure mode is exited. For example, in the safety mode, the control device may update the motion limit of the execution arm to the target pose of the end of the execution arm when it is determined that the target pose of the end of the execution arm is outside the working space of the execution arm, and then cause the robot system to exit the safety mode to continue performing the operation. Or in the teleoperation process, the control device can automatically disconnect or prompt the user to disconnect the master-slave mapping between the master manipulator and the execution arm, and after the user adjusts the master manipulator to a proper position, the master-slave matching is completed again, the master-slave mapping is established, and then the safety mode is exited.
In some embodiments, the method 500 may further comprise: and determining the pose of the tail end coordinate system of the execution arm relative to the reference coordinate system based on the two-dimensional coordinates of the plurality of pose identification pattern corner points in the positioning image, the three-dimensional coordinates of the plurality of pose identification pattern corner points in the tail end coordinate system of the execution arm and the transformation relation of the camera coordinate system relative to the reference coordinate system. In some embodiments, the transformation of the camera coordinate system with respect to the reference coordinate system may be known. For example, the reference coordinate system is a world coordinate system, and the transformation relationship between the camera coordinate system and 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, based on the camera imaging principle and the projection model, the pose of the end coordinate system of the execution arm relative to the camera coordinate system is determined based on the two-dimensional coordinates of the plurality of pose identification pattern corner points in the positioning image and the three-dimensional coordinates of the plurality of pose identification pattern corner points in the end coordinate system of the execution arm. Based on the transformation relation between the pose of the end coordinate system of the execution arm relative to the camera coordinate system and the transformation relation between the camera coordinate system relative to the reference coordinate system, the pose of the end coordinate system of the execution arm relative to the reference coordinate system can be obtained. In some embodiments, camera intrinsic parameters may also be considered. For example, the camera's internal parameters may be the camera's internal parameters 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 camera origin. 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 left and right lens center line (e.g., the midpoint of the line).
In some embodiments, the pose of the end coordinate system { wm } of the actuator arm relative to the reference coordinate system (e.g., world coordinate system) { w } may be determined based on the following equation (3):
wherein, w R wm to perform the pose of the end coordinate system of the arm with respect to the reference coordinate system, w P wm to perform the position of the end coordinate system of the arm relative to the reference coordinate system, w R lens for the pose of the camera coordinate system relative to the reference coordinate system, w P lens for the position of the camera coordinate system relative to the reference coordinate system, lens R wm to perform the pose of the end coordinate system of the arm with respect to the camera coordinate system, lens P wm to position the end coordinate system of the actuator arm relative to the camera coordinate system.
Some embodiments of the present disclosure provide methods of determining three-dimensional coordinates of a plurality of pose identifiers relative to an end coordinate system of an implement arm. In some embodiments, three-dimensional coordinates of the plurality of pose identifiers relative to an end coordinate system of the actuator arm are determined based on a distribution of the plurality of pose identifiers. For example, three-dimensional coordinates of the plurality of pose identification pattern corner points in the end coordinate system of the actuator arm are determined based on the distribution of the plurality of pose identification pattern corner points.
FIG. 10 illustrates a flowchart of a method 1000 of determining three-dimensional coordinates of a plurality of pose identifiers relative to an end coordinate system of an implement arm according to some embodiments of the present disclosure. As shown in fig. 10, method 1000 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 1000 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). 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 (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 1000. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to FIG. 10, at step 1001, based on a distribution of the plurality of pose markers, an axis-wrapping angle of the plurality of pose markers with respect to a Z-axis of an end coordinate system of the actuator arm is determined. In some embodiments, an axis-wrapping angle of the plurality of pose markers relative to a Z-axis of an end coordinate system of the actuator arm may be determined based on the plurality of pose marker patterns. For example, each pose identification pattern may identify a particular pivot angle, with different pose identification patterns corresponding one-to-one to the identified pivot angle. Based on the recognized pose identification pattern and the correspondence of the pose identification pattern and the pivot angle, the pivot angle identified by the recognized pose identification pattern can be determined. It should be appreciated that the distribution of each pose identification pattern is known or predetermined. In some embodiments, the plurality of pose identification patterns or the distribution of the plurality of pose identification pattern angular points may be a distribution as shown in fig. 3. In some embodiments, the pivot angle of each pose identification pattern corner identification may also be determined based on equation (1).
Referring to FIG. 10, at step 1003, three-dimensional coordinates of the plurality of pose markers relative to an end coordinate system of the actuator arm are determined based on the pivot angles of the plurality of pose markers. In some embodiments, as shown in fig. 4, each pose identification pattern corner is located on the circumference of a cross-sectional circle 420, and the center of the cross-sectional circle 420 and the radius r are known. Identifying pattern corner points P by pose 4 As reference corner point, pose identification pattern corner point P 4 The three-dimensional coordinates of { wm } in the end coordinate system of the actuator arm are (r, 0). In some embodiments, the three-dimensional coordinates of each pose identification pattern corner in the end coordinate system { wm } of the execution arm may be determined based on the following equation (4):
C m =[r·cosα m r·sinα m 0] T (4)
wherein C is m To identify pattern corner P with pose 7 As the first pose identification pattern corner, according to the clockwise direction of the cross-sectional circle 720, the specific pivot angle identified by the mth pose identification pattern corner may be based on three-dimensional coordinates of the plurality of pose identifications in the end coordinate system { wm } of the execution arm.
In some embodiments, the first is determined based on equation (1)Around-axis angle alpha of m pose identification pattern corner identifications m The pivot angle α is then determined based on equation (1) m And equation (4) determining the three-dimensional coordinate C m 。
FIG. 11 illustrates a flow chart of a method 1100 of determining three-dimensional coordinates of a plurality of pose identifiers relative to an end coordinate system of an implement arm according to further embodiments of the present disclosure. Method 1100 may be an alternative embodiment of method 1000 shown in fig. 10. As shown in fig. 11, method 1100 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 1100 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). In some embodiments, 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 (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 1100. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to fig. 11, in step 1101, an arrangement order of the plurality of pose identifiers is determined based on at least two of the plurality of pose identifiers. In some embodiments, the arrangement order of the plurality of pose identifiers may be represented by the arrangement order of the plurality of pose identifier patterns. In some embodiments, the order of arrangement of the plurality of pose identifiers is determined by identifying any two pose identifier patterns. It should be appreciated that the plurality of pose identifiers includes different pose identifier patterns, and that in the case where any two pose identifier patterns are known, an arrangement order of the plurality of pose identifiers in the positioning image, such as a clockwise arrangement or a counterclockwise arrangement, may be determined based on a distribution of the plurality of pose identifier patterns known (e.g., a distribution of the different pose identifier patterns in the tag 300 shown in fig. 3, or a distribution of the different pose identifier patterns in the tag 400 shown in fig. 4).
Referring to FIG. 11, at step 1103, three-dimensional coordinates of the plurality of pose identifiers with respect to an end coordinate system of the actuator arm are determined based on an arrangement order of the plurality of pose identifiers. In some embodiments, based on a known distribution of the plurality of pose identifiers, three-dimensional coordinates of each pose identifier in the end coordinate system of the actuator arm may be determined, the three-dimensional coordinates of each pose identifier may be represented by three-dimensional coordinates of the pose identifier pattern corner points in the end coordinate system of the actuator arm, each pose identifier pattern corresponding to one coordinate point in the end coordinate system of the actuator arm. After determining the arrangement order of the plurality of pose identification patterns, the rest of the pose identification patterns can be determined based on the identified pose identification patterns, and then three-dimensional coordinates of each pose identification pattern in the end coordinate system of the execution arm can be determined. In some embodiments, a plurality of pose identification corner points in the positioning image are identified, and any two corresponding pose identification patterns in the plurality of pose identification corner points are determined. And determining the arrangement sequence of the plurality of pose identification pattern corner points based on the two recognized pose identification patterns, so that the three-dimensional coordinates of each pose identification pattern corner point in the tail end coordinate system of the execution arm can be determined. In addition, based on the arrangement sequence, the distribution of all the pose identification patterns can be determined, so that the pose identification patterns at the corresponding positions on the positioning image are matched by using a specific pose pattern matching template, and the data processing speed is improved. In some embodiments, pattern matching at pose pattern corner points with pose identification pattern templates may be implemented similarly to step 1203 in method 1200.
Some embodiments of the present disclosure provide methods of identifying pose identifiers. Fig. 12 illustrates a flowchart of a method 1200 of identifying pose identifications according to some embodiments of the present disclosure. As shown in fig. 12, method 1200 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 1200 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). In some embodiments, method 1200 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 1200. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to fig. 12, at step 1201, a plurality of candidate pose identifications are determined from a localization image. In some embodiments, the pose identification may include pose identification pattern corner points in the pose identification pattern. The coordinates of the candidate pose identification or the origin of the coordinate system can be represented by the candidate pose identification pattern corner points. In some embodiments, the candidate pose identification pattern corner points may refer to possible pose identification pattern corner points obtained through preliminary processing or preliminary recognition of the positioning image.
In some embodiments, the method 1200 may further include determining a region of interest (Region of Interest, ROI) in the localization image. For example, the ROI may be first truncated from the localization image, and a plurality of candidate pose identifications may be determined from the ROI. The ROI may be a whole image of the positioning image or a partial region. For example, the ROI of the current frame may be truncated based on a plurality of regions within a range of pose identification pattern corner points determined from the previous frame image (e.g., the positioning image of the previous image processing cycle). For the positioning image of the non-first frame, the ROI may identify, for a plurality of poses of the previous image processing cycle, a region within a certain distance range centered on a virtual point constituted by coordinates of the pattern corner points. The certain distance range may be a fixed multiple, for example twice, of the average separation distance of the pose identification pattern corner points. It should be appreciated that the predetermined multiple may also be a variable multiple of the average separation distance of the plurality of candidate pose identification pattern corner points in the previous image processing cycle.
In some embodiments, the method 1200 may further include determining corner likelihood values (Corner Likelihood, CL) for each pixel point in the positioning image. In some embodiments, the corner likelihood values for the pixel points may be numerical values that characterize the likelihood of the pixel points as feature points (e.g., corner points). In some embodiments, the positioning image may be preprocessed before computing the corner likelihood values for each pixel, after which the corner likelihood values for each pixel in the preprocessed image are determined. The preprocessing of the image may include, for example: at least one of image graying, image denoising and image enhancement. For example, image preprocessing may include: and cutting the ROI from the positioning image, and converting the ROI into a corresponding gray image.
In some embodiments, determining the corner likelihood value of each pixel in the ROI may include, for example, convolving each pixel in the ROI to obtain a first and/or second derivative of each pixel. And (3) obtaining the corner likelihood value of each pixel point by using the first-order derivative and/or the second-order derivative of each pixel point in the ROI range. Illustratively, the corner likelihood values for each pixel may be determined based on the following equation (5):
where τ is a set constant, for example, set to 2; i x 、I 45 、I y 、I n45 The first derivatives of the pixel points in the directions of 0, pi/4, pi/2 and pi/4 are respectively shown; i xy And I 45_45 The second derivatives of the pixel points in the 0, pi/2 and pi/4, -pi/4 directions, respectively.
In some embodiments, the method 1200 may further include dividing the ROI into a plurality of sub-regions. For example, a non-maximal suppression method may be used to equally divide multiple sub-images in a ROI range. In some embodiments, the ROI may be divided equally into multiple sub-images of 5×5 pixels. The above-described embodiments are exemplary and not limiting, and it should be appreciated that the positioning image or ROI may also be segmented into multiple sub-images of other sizes, for example, into multiple sub-images of 9 x 9 pixels.
In some embodiments, method 1200 may further include determining a pixel in each sub-region for which the corner likelihood value is greatest to form a set of pixels. In some embodiments, the set of pixels is identified as a plurality of candidates determined from the localization image. For example, a pixel point with the largest CL value in each sub-image may be determined, and the pixel point with the largest CL value in each sub-image may be compared with a first threshold value to determine a set of pixels with CL values greater than the first threshold value. In some embodiments, the first threshold may be set to 0.06. It should be appreciated that the first threshold value may also be set to other values.
Referring to fig. 12, step 1203 identifies a first pose identifier from a plurality of candidate pose identifiers based on a plurality of different pose pattern matching templates. In some embodiments, a plurality of different pose pattern matching templates are respectively matched with patterns at candidate pose identification pattern corner points to identify a first pose identification. For example, candidate pose identification pattern corner points reaching a preset pose pattern matching degree standard are determined as first pose identification pattern corner points. In some embodiments, the pose pattern matching template has the same or similar features as the pattern of the region near the pose identification pattern corner. If the matching degree of the pose pattern matching template and the pattern of the region near the candidate pose identification pattern corner reaches the preset pose pattern matching degree standard (for example, the matching degree is higher than a threshold value), the pattern near the candidate pose identification pattern corner can be considered to have the same or similar characteristics as the pose pattern matching template, and then the current candidate pose identification pattern corner can be considered to be the pose identification pattern corner.
In some embodiments, a pixel point with the largest CL value in the pixel set is determined as a candidate pose identification pattern corner. For example, all pixels in the pixel set may be ordered in order of CL values from high to low, and the pixel with the highest CL value may be used as the candidate pose identification pattern corner. In some embodiments, after determining the candidate pose identification pattern corner, matching the pose pattern matching template with the pattern at the candidate pose identification pattern corner, and if a preset pose pattern matching degree standard is reached, determining the candidate pose identification pattern corner as the identified first pose identification pattern corner.
In some embodiments, method 1200 may further include determining, as the candidate pose identification pattern corner, a pixel having a greatest corner likelihood value for remaining pixels in the set of pixels in response to a match failure. For example, if the candidate pose identification pattern corner does not reach the preset matching degree standard, selecting a pixel point with a secondary CL value (a pixel point with a second largest CL value) as the candidate pose identification pattern corner, matching the pose pattern matching template with a pattern at the candidate pose identification pattern corner, and so on until the first pose identification pattern corner is identified.
In some embodiments, the pose identification patterns may be black and white alternate patterns (e.g., checkerboard patterns), so the pose pattern matching templates may be the same patterns, utilizing the gray distribution G of the pose pattern matching templates M Pixel neighborhood gray scale distribution G of pixel point corresponding to candidate pose identification pattern corner point image The correlation coefficients (Correlation Coefficient, CC) between the two are matched. Pixel neighborhood gray scale distribution G of pixel point image The gradation distribution of pixels is a constant range (for example, 10×10 pixels) of pixels centered on the pixel point. The correlation coefficient may be determined based on the following equation (6):
where Var () is a variance function and Cov () is a covariance function. In some embodiments, when the correlation coefficient is smaller than 0.8, the correlation between the gray distribution in the pixel domain and the pose pattern matching template is lower, and then the candidate pose identification pattern corner with the largest corner likelihood value is judged to be not the pose identification pattern corner, otherwise, the candidate pose identification pattern corner with the largest corner likelihood value is considered to be the pose identification pattern corner.
In some embodiments, the method 1200 may further include determining an edge direction of the candidate pose identification pattern corner. For example, as shown in fig. 13, the candidate pose identification pattern corner is corner P in pose identification pattern 1300 13 Corner point P 13 The edge direction of (a) may refer to forming the corner point P 13 As indicated by the dashed arrow in fig. 13.
In some embodiments, the edge direction may be determined by determining, for each pixel of a range neighborhood (e.g., 10X 10 pixels) centered at the candidate pose identification pattern corner, the first derivative values (I x And I y ) And (5) determining. For example, the edge direction may be calculated based on the following formula (7):
wherein the first derivative (I x And I y ) Can be obtained by carrying out convolution operation on each pixel point in a certain range neighborhood range. In some embodiments, the edge direction I of the pixel points in each range neighborhood is determined by angle And corresponding weight I weight Clustering calculation is carried out to obtain the edge direction of the pixel point, and the weight I is selected weight Class-corresponding I with maximum duty cycle angle As the edge direction. If there are a plurality of edge directions, the weight I is selected weight I corresponding to multiple classes with maximum duty ratio angle As the edge direction.
In some embodiments, the method used for the cluster computation may be any one of a K-means method, a BIRCH (Balanced Iterative Reducing and Clustering using Hierarchies, hierarchical structure based balanced iterative clustering method) method, a DBSCAN (Density-Based Spatial Clustering of Applications with Noise, density based clustering method with noise) method, a GMM (Gaussian Mixed Model, gaussian mixture model) method.
In some embodiments, the method 1200 may further include rotating the pose pattern matching template based on the edge direction. According to the edge direction rotation pose pattern matching template, the pose pattern matching template can be aligned with an image at the candidate pose identification pattern corner point. The edge direction of the candidate pose identification pattern corner may be used to determine the arrangement direction of the image at the candidate identification pattern corner in the positioning image. In some embodiments, the pose pattern matching template may be adjusted to be the same or nearly the same as the image direction at the candidate pose identification pattern corner points in order to facilitate image matching according to the edge direction rotation pose pattern matching template.
Referring to fig. 12, step 1205 searches for a pose identifier starting from a first pose identifier. For example, fig. 14 shows a flowchart of a method 1400 for searching for pose identification according to some embodiments of the present disclosure. As shown in fig. 14, method 1400 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 1400 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). In some embodiments, method 1400 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control device 140 shown in fig. 1, control device 220 shown in fig. 2, or control device 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 1400. In some embodiments, these instructions may be stored on a computer readable medium.
Referring to fig. 14, in step 1401, a second pose identifier is searched for starting from the first pose identifier. In some embodiments, the second pose identification pattern corner is searched in the set search direction with the first pose identification pattern corner as a starting point. In some embodiments, the set search direction may include: the first pose identifies at least one direction of a right front (corresponding to an angular direction of 0 °), a right rear (corresponding to an angular direction of 120 °), a right upper (angular direction of 90 °), a right lower (-angular direction of 90 °) and an oblique direction (e.g., an angular direction of ±45°).
In some embodiments, the set search direction is n, e.g., searching in 8 directions, each search direction v sn Can be determined based on the following equation (8):
v sn =[cos(n·π/4)sin(n·π/4)],(n=1,2,…,8) (8)
in some embodiments, the search direction set in the current step may be determined according to a deviation angle between adjacent pose identification pattern corner points among the pose identification pattern corner points determined in the previous frame. Illustratively, the predetermined search direction is determined based on the following equation (9):
wherein, (x) j ,y j ) Identifying two-dimensional coordinates of pattern corner points for a plurality of poses determined for a previous frame (or a previous image processing period); n is n last Identifying the number of pattern corner points for a plurality of poses determined by the previous frame; v s1 A search direction set for the first one; v s2 A search direction set for the second.
In some embodiments, as shown in FIG. 15, pattern corner P is identified in a first pose 1501 Is used as a searching starting point, and a second pose identification pattern corner point P is searched in a set searching direction 1502 The coordinate positions of (2) may specifically include: identifying pattern corner P with first pose 1501 Is used as a search start point in a set search direction V with a certain search step by a search box (for example, a broken line box in fig. 15) 1501 And (5) searching pose identification pattern corner points. If at least one candidate pose identification pattern corner exists in the search frame, the candidate pose identification pattern corner with the maximum likelihood value of the corner in the search frame is preferentially selected as the second pose identification pattern corner P 1502 . Under the condition that the search box is limited to a proper size, the pattern corner point P is marked by a first pose 1501 Is used as a searching starting point to carry out the second pose identification pattern corner point P 1502 When searching, the candidate pose identification pattern corner with the largest likelihood value among the candidate pose identification pattern corner appearing in the search frame is more likely to be the pose identification pattern corner. Therefore, the candidate pose identification pattern corner point with the maximum likelihood value in the search frame can be considered as the second pose identification pattern corner point P 1502 In order to increase the data processing speed. In other embodiments, in order to improve accuracy of identifying pose identification pattern corner points, candidate pose identification pattern corner points with the largest likelihood value of corner points in candidate pose identification pattern corner points appearing in a search frame are selected for identifying the corner points, so as to determine whether the candidate pose identification pattern corner points with the largest likelihood value of the corner points are the pose identification pattern corner points. For example, matching the pose pattern matching template with the image within a certain range at the candidate pose identification pattern corner point with the maximum likelihood value of the corner point, so as to meet the preset pose pattern matching degree standardMay be considered as the searched second pose identification pattern corner P 1502 。
In some embodiments, with continued reference to fig. 15, 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 be varied in synchronization with the side length of the search box. In other embodiments, the size of the search box may be a fixed size.
In some embodiments, the pose identification pattern may be a black and white checkerboard pattern, and pattern matching may be performed based on the correlation coefficient in equation (6). If the correlation coefficient is larger than the threshold value, the candidate pose identification pattern corner with the maximum likelihood value of the corner is considered to be the pose identification pattern corner, and the candidate pose identification pattern corner is marked as the second pose identification pattern corner.
Fig. 16 illustrates a flowchart of a method 1600 for searching for a second pose identification according to some embodiments of the present disclosure. As shown in fig. 16, method 1600 may be implemented or performed by hardware, software, or firmware. In some embodiments, method 1600 may be performed by a robotic system (e.g., robotic system 100 shown in fig. 1, or robotic system 1900 shown in fig. 19). In some embodiments, method 1600 may be implemented as computer readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform method 1600. In some embodiments, these instructions may be stored on a computer readable medium. In some embodiments, step 1401 in method 1400 may be implemented similarly to method 1600.
Referring to fig. 16, in step 1601, candidate pose identification pattern corner points of the second pose identification are searched for using the first pose identification as a starting point. In some embodiments, searching for candidate pose identification pattern corner points for the second pose identification may be performed in conjunction with searching for second pose identification pattern corner points P shown in fig. 15 1502 Similarly implemented.
In step 1603, based on the distribution of the plurality of pose identifiers, a first pose pattern matching template and a second pose pattern matching template are determined, the first pose pattern matching template and the second pose pattern matching template corresponding to pose identifiers adjacent to the first pose identifier. In some embodiments, step 1603 may be performed before or after step 1601, and step 1603 may also be performed in synchronization with step 1601. In some embodiments, the pose identification patterns included in the pose identifications adjacent to the first pose identification may be determined based on the pose identification pattern included in the first pose identification and the distribution of the plurality of pose identification patterns, thereby determining a first pose pattern matching template and a second pose pattern matching template.
In step 1605, the first pose pattern matching template and/or the second pose pattern matching template is matched with the pattern at the candidate pose identification pattern corner location of the second pose identification to identify the second pose identification. In some embodiments, the first pose pattern matching template and/or the second pose pattern matching template may be matched with the pattern at the candidate pose identification pattern corner position of the second pose identification based on the correlation coefficient in equation (6). If the correlation coefficient is greater than the threshold value, determining candidate pose identification pattern corner points of the second pose identification as pose identification pattern corner points of the second pose identification, and determining a pose pattern matching template (a first pose pattern matching template or a second pose pattern matching template) with the correlation coefficient greater than the threshold value as the pose identification pattern of the second pose identification.
Referring to fig. 14, in step 1403, a search direction is determined based on the first pose identification, the second pose identification. In some embodiments, the search direction includes: a first search direction and a second search direction. The first search direction may be a direction which takes a coordinate position of the first pose identification pattern corner as a starting point and is far away from the second pose identification pattern corner. The second search direction may be a direction which starts from the coordinate position of the second pose identification pattern corner and is away from the first pose identification pattern corner. For example, the search direction V shown in fig. 15 1502 。
Referring to fig. 14, in step 1405, either the first pose or the second pose is identifiedThe identity serves as a starting point, and the pose identity is searched in the searching direction. In some embodiments, if the first pose identification pattern corner 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 of the pose identification pattern corner. If the second pose identification pattern corner is taken as a new searching starting point, the second searching direction in the above embodiment is taken as a searching direction to search the pose identification pattern corner. In some embodiments, a new pose identification pattern corner is searched (e.g., the third pose identification pattern corner P in fig. 15 1503 ) May be performed similar to step 1401 or method 1700 in method 1400. In some embodiments, the search step may be the first pose identification pattern corner P 1501 And a second pose identification pattern corner point P 1502 Distance L between 1 。
Fig. 17 illustrates a flowchart of a method 1700 for searching for pose identification according to some embodiments of the present disclosure. As shown in fig. 17, method 1700 may be implemented or performed by hardware, software, or firmware. In some embodiments, the method 1700 may be performed by a robotic system (e.g., the robotic system 100 shown in fig. 1, or the robotic system 1900 shown in fig. 19). In some embodiments, the method 1700 may be implemented as computer-readable instructions. These instructions may be read and executed by a general purpose processor or a special purpose processor (e.g., control 120 shown in fig. 1, control 220 shown in fig. 2, or control 1920 shown in fig. 19). For example, a control device for a robotic system may include a processor configured to perform the method 1700. In some embodiments, these instructions may be stored on a computer readable medium. In some embodiments, step 1405 in method 1400 may be implemented similarly to method 1700.
Referring to fig. 17, in step 1701, candidate pose identification pattern corner points of the third pose identification are searched using the first pose identification or the second pose identification as a starting point. In some embodiments, searching for candidate pose identification pattern corner points for third pose identification may be performed in comparison to searching for third pose identification pattern corner points P shown in fig. 15 1503 Similarly implemented.
In step 1703, a third pose pattern matching template is determined based on the distribution of the plurality of pose identifiers, the third pose pattern matching template corresponding to a pose identifier adjacent to the first pose identifier or adjacent to the second pose identifier. In some embodiments, the pose identification pattern included in the pose identification adjacent to the first pose identification or the second pose identification can be determined based on the pose identification pattern included in the first pose identification or the second pose identification and the distribution of the plurality of pose identification patterns, and further, the pose identification pattern included in the pose identification adjacent to the first pose identification or the second pose identification can be determined, so that a third pose pattern matching template can be determined.
In step 1705, the third pose pattern matching template is matched with the pattern at the candidate pose identification pattern corner position of the third pose identification to identify the third pose identification. In some embodiments, step 1705 may be implemented similarly to step 1605.
In some embodiments, in response to the search distance being greater than the search distance threshold, determining a pixel of the set of pixels having a maximum likelihood value for a corner of the remaining pixels as a candidate pose identification pattern corner; and matching the plurality of different pose pattern matching templates with the patterns at the corner positions of the candidate pose identification patterns respectively so as to identify the first pose identification. In some embodiments, after determining the pixel with the largest corner likelihood value for the remaining pixels in the set of pixels as the new candidate pose identification pattern corner, a new first pose identification may be identified based on a method similar to step 1203. In some embodiments, the search distance being greater than the search distance threshold may be understood as the search distance in some or all of the search directions being greater than the search distance threshold. In some embodiments, the search distance threshold may include a set multiple of the distances of the N-1 th pose identification pattern corner and the N-2 nd pose identification pattern corner, where N+.3.
For example, the first two poses, whose distance threshold is doubled, identify the distance of the pattern corner points. In this way, the maximum searching distance for searching the corner point of the third pose identification pattern is twice the distance between the corner points of the first pose identification pattern and the corner points of the second pose identification pattern, if the searching distance is reached in the searching direction and the corner points of the pose identification pattern are not searched, the pixel with the maximum likelihood value of the corner points of the rest pixels in the pixel set is determined to be used as the corner point of the new candidate pose identification pattern, the new first pose identification is identified, and the current searching process is correspondingly stopped. In some embodiments, similar to method 1200, the new first pose identification pattern corner may be redetermined, and similar to method 1400, the remaining pose identification pattern corner may be searched with the new pose identification pattern corner as a search starting point.
In some embodiments, in response to the number of identified pose identification pattern corner points being greater than or equal to the pose identification number threshold, a pose of the end of the execution arm relative to the reference coordinate system may be determined based on the searched pose identification search, with the search of the pose identification pattern corner points also stopping accordingly. For example, when four pose identification pattern corner points are identified, the search for the pose identification pattern corner points is stopped.
In some embodiments, in response to the identified number of pose identifications being less than the threshold number of pose identifications, determining a pixel in the set of pixels having a maximum likelihood value for a corner of the remaining pixels as a candidate pose identification pattern corner; and matching the plurality of different pose pattern matching templates with the patterns at the corner positions of the candidate pose identification patterns respectively so as to identify the first pose identification. In some embodiments, if the total number of recognized pose identifiers (e.g., pose identifier pattern corner points) is less than the set pose identifier number threshold, the search based on the first pose identifier in the above step is considered to fail. In some embodiments, in the event of a search failure, the pixel with the largest likelihood value for the corner of the remaining pixels in the set of pixels is determined as the new candidate pose identification pattern corner, after which the new first pose identification may be identified based on a method similar to step 1203. In some embodiments, similar to method 1200, the new first pose identification pattern corner may be redetermined, and similar to method 1400, the remaining pose identification pattern corner may be searched with the new pose identification pattern corner as a search starting point.
In some embodiments, after the pose identification pattern corner is searched or identified, the determined pose identification pattern corner can be subjected to sub-pixel positioning so as to improve the position accuracy of the pose identification pattern corner.
In some embodiments, CL values of the pixel points may be fitted based on a model to determine coordinates of the sub-pixel located pose identification pattern corner points. For example, the fitting function of CL values for each pixel point in the ROI may be a quadric function, whose extreme points are sub-pixel points. The fitting function may be determined based on the following formulas (10) and (11):
S(x,y)=ax 2 +by 2 +cx+dy+exy+f (10)
wherein S (x, y) is a CL value fitting function of all pixel points in each ROI, a, b, c, d, e, f is a coefficient; x is x c The x-coordinate, y, identified for pose c The y-coordinate identified for the pose.
In some embodiments of the present disclosure, the present disclosure also provides a computer device including a memory and a processor. The memory may be used to store at least one instruction and the processor coupled to the memory for executing the at least one instruction to perform some or all of the steps in the methods of the present disclosure, such as some or all of the steps in the methods disclosed in fig. 5-12, 14, 16, and 17.
Fig. 18 illustrates a schematic block diagram of a computer device 1800 in accordance with some embodiments of the present disclosure. With reference to FIG. 18, the computer device 1800 may include a Central Processing Unit (CPU) 1801, a system memory 1804 including a Random Access Memory (RAM) 1802 and a Read Only Memory (ROM) 1803, and a system bus 1805 connecting the various components. The computer device 1800 may also include an input/output system 1806, and a mass storage device 1807 for storing an operating system 1813, application programs 1814, and other program modules 1815. The input/output system 1806 is mainly composed of a display 1808 and an input device 1809.
The mass storage device 1807 is connected to the central processing unit 1801 through a mass storage controller (not shown) connected to the system bus 1805. The mass storage device 1807 or computer readable medium provides non-volatile storage for the computer device. The mass storage device 1807 may include a computer-readable medium (not shown) such as a hard disk or a compact disk-read Only Memory (CD-ROM) drive.
Computer readable media may include computer storage media and communication media without loss of generality. 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 memory 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 recognize that computer storage media are not limited to the ones described above. The above-described system memory and mass storage devices may be collectively referred to as memory.
The computer device 1800 may connect to the network 1812 through a network interface unit 1811 connected to the system bus 1805.
The system memory 1804 or mass storage device 1807 also is used to store one or more instructions. The central processing unit 1801 implements all or part 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 that is executable by a processor to cause a computer to perform some or all of the steps in the methods of some embodiments of the present disclosure, such as some or all of the steps in the methods disclosed in fig. 5-12, 14, 16, and 17. Examples of the computer-readable storage medium include memories of computer programs (instructions), such as Read-Only Memory (ROM), random access Memory (Random Access Memory, RAM), compact disc-Only Memory (CD-ROM), magnetic tapes, floppy disks, optical data storage devices, and the like.
Fig. 19 illustrates a schematic diagram of a robotic system 1900 according to some embodiments of the disclosure. In some embodiments of the present disclosure, referring to fig. 19, a robotic system 1900 may include: the tool 1950, the driving device 1960, the image capturing apparatus 1910, and the control device 1920 (e.g., a processor), the tool 1950 may include an actuator arm 1940 and a tip 1930 disposed at a distal end of the actuator arm 1940. A plurality of pose markers may be formed or disposed on the distal end 1930, including different pose marker patterns, and an actuator may be disposed at the distal end of the distal end 1930. A drive 1960 may be used to control the pose of the actuator arm 1940 and its distal end 1930, and drive sensors are coupled to the drive 1960 and used to obtain drive information. Image acquisition device 1910 may be used to acquire positioning images of the implement arm 1940. The control 1920 is coupled to the drive 1960, the drive sensor, and the image capture device 1910 for performing some or all of the steps in the methods of some embodiments of the present disclosure, such as some or all of the steps in the methods disclosed in fig. 5-12, 14, 16, and 17.
Note that the above is merely exemplary embodiments of the present disclosure and the technical principles applied. Those skilled in the art will appreciate that the present disclosure is not limited to the particular embodiments described herein, and that various obvious changes, rearrangements and substitutions can be made by those skilled in the art without departing from the scope of the disclosure. Therefore, while the present disclosure has been described in connection with the above embodiments, the present disclosure is not limited to the above embodiments, but may include many 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 (24)
1. A robot system fault detection processing method, wherein the robot system includes a control device, at least one driving device, and an execution arm, the at least one driving device driving the execution arm based on a driving signal sent by the control device, the method comprising:
determining a target pose of the end of the execution arm;
acquiring a positioning image;
identifying, in the positioning image, a plurality of pose identifiers located on the end of the execution arm, the plurality of pose identifiers comprising different pose identifier patterns;
Determining an actual pose of the end of the execution arm based on the plurality of pose identifiers;
based on the target pose and the actual pose meeting error conditions, enabling the robot system to enter a safe mode;
determining a fault condition of the robotic system; and
performing a fault recovery operation in response to determining that the fault condition of the robotic system is a recoverable fault; or (b)
Responsive to determining that the fault condition of the robotic system is an unrecoverable fault, causing the robotic system to enter an exit mode;
identifying a plurality of pose identifiers located on a distal end of the execution arm includes:
determining a plurality of candidate pose identifiers from the positioning image;
identifying a first pose identifier from the plurality of candidate pose identifiers based on a plurality of different pose pattern matching templates; and
searching other pose identifiers by taking the first pose identifier as a starting point;
determining the actual pose of the tip of the actuator arm includes:
based on the distribution of the plurality of pose identifiers, determining the axis-surrounding angles of the plurality of pose identifiers relative to the Z axis of the tail end coordinate system of the execution arm;
based on the pivot angles of the plurality of pose identifiers, determining three-dimensional coordinates of the plurality of pose identifiers relative to an end coordinate system of the execution arm;
Determining two-dimensional coordinates of the plurality of pose identifiers in the positioning image; and
and determining the pose of the tail end coordinate system of the execution arm relative to the reference coordinate system based on the two-dimensional coordinates of the plurality of pose marks in the positioning image and the three-dimensional coordinates of the plurality of pose marks relative to the tail end coordinate system of the execution arm, and taking the pose as the actual pose.
2. The method of claim 1, wherein causing the robotic system to enter a safe mode comprises:
deactivating the at least one drive device that drives the actuator arm; and
allowing a user of the robotic system to use a manual mode.
3. The method as recited in claim 1, further comprising:
responsive to the failure recovery operation failing, causing the robotic system to enter the exit mode; or (b)
And in response to the failure recovery operation being successful, causing the robotic system to exit the safe mode.
4. The method according to claim 1, wherein the robot system includes a communication node for relaying the driving signal transmitted by the control device and a communication module for communicating with the communication node,
The method further comprises the steps of:
transmitting an inquiry signal to the communication node; and
and generating a communication fault alarm signal indicating the occurrence of communication faults and/or restarting the communication module in response to the failure to receive the response signal of the communication node.
5. The method as recited in claim 1, further comprising:
in response to failure to receive status information from the at least one drive device, generating a communication failure alarm signal indicating occurrence of a communication failure and/or restarting a communication module for communication with the at least one drive device.
6. The method according to claim 4 or 5, further comprising:
and in response to the communication failure not being recovered after the restart, enabling the robot system to enter an exit mode.
7. The method as recited in claim 1, further comprising:
receiving status information of the at least one driving device; and
and generating a driving device fault alarm signal in response to the state information and the driving signal of the at least one driving device sent by the control device to meet a driving device fault condition, wherein the driving device fault alarm signal indicates that the driving device of the execution arm is in fault.
8. The method as recited in claim 7, further comprising:
and generating an execution arm fault alarm signal and/or enabling the robot system to enter an exit mode in response to the state information and the driving signal of the at least one driving device sent by the control device not meeting a driving device fault condition, wherein the execution arm fault alarm signal indicates that an execution arm fault occurs.
9. The method as recited in claim 1, further comprising: judging whether the target pose exceeds the motion limit of the execution arm;
generating an alarm signal indicating that the target pose exceeds a motion limit of the actuator arm in response to the target pose exceeding the motion limit; and
the secure mode is exited.
10. The method as recited in claim 1, further comprising:
receiving actual joint information of at least one joint of the execution arm;
determining target joint information of at least one joint of the execution arm based on the target pose; and
and responding to the actual joint information and the target joint information to meet an execution arm fault condition, generating an execution arm fault alarm signal and/or enabling the robot system to enter an exit mode, wherein the execution arm fault alarm signal indicates that an execution arm fault occurs.
11. The method of claim 1, wherein determining a target pose of the tip of the actuator arm comprises:
the target pose is determined in advance or randomly generated; or alternatively
The target pose is determined based on the pose of the main operator.
12. The method of claim 1, wherein the pose identification comprises pose identification pattern corner points in the pose identification pattern, the method comprising:
determining a region of interest in the localization image;
dividing the region of interest into a plurality of sub-regions;
determining the pixel with the maximum likelihood value of the corner in each sub-region to form a pixel set;
determining a pixel with the maximum likelihood value of the corner in the pixel set as a candidate pose identification pattern corner; and
and matching the plurality of different pose pattern matching templates with the patterns at the corner positions of the candidate pose identification patterns respectively so as to identify the first pose identification.
13. The method according to claim 12, comprising:
and responding to the matching failure, and determining the pixel with the maximum corner likelihood value of the rest pixels in the pixel set as the candidate pose identification pattern corner.
14. The method of any one of claims 1, 12, 13, further comprising:
searching a second pose mark by taking the first pose mark as a starting point;
determining a search direction based on the first pose identifier and the second pose identifier; and
and searching other pose identifiers in the searching direction by taking the first pose identifier or the second pose identifier as a starting point.
15. The method of claim 14, wherein searching for a second pose identifier with the first pose identifier as a starting point comprises:
searching candidate pose identification pattern corner points of the second pose identification by taking the first pose identification as a starting point;
determining a first pose pattern matching template and a second pose pattern matching template based on the distribution of the plurality of pose identifiers, wherein the first pose pattern matching template and the second pose pattern matching template correspond to the pose identifiers adjacent to the first pose identifier; and
and matching the first pose pattern matching template or the second pose pattern matching template with the pattern at the candidate pose identification pattern corner position of the second pose identification so as to identify the second pose identification.
16. The method of claim 14, wherein searching for other pose identifiers in the search direction with the first pose identifier or the second pose identifier as a starting point comprises:
searching candidate pose identification pattern corner points of a third pose identification by taking the first pose identification or the second pose identification as a starting point;
determining a third pose pattern matching template based on the distribution of the plurality of pose identifiers, the third pose pattern matching template corresponding to a pose identifier adjacent to the first pose identifier or adjacent to the second pose identifier; and
and matching the third pose pattern matching template with the pattern at the corner position of the candidate pose identification pattern of the third pose identification to identify the third pose identification.
17. The method as recited in claim 14, further comprising:
determining a pixel with the maximum likelihood value of the corner points of the rest pixels in the pixel set as a candidate pose identification pattern corner point in response to the search distance being greater than a search distance threshold; and
and matching the plurality of different pose pattern matching templates with the patterns at the corner positions of the candidate pose identification patterns respectively so as to identify the first pose identification.
18. The method as recited in claim 14, further comprising:
determining the pixel with the maximum likelihood value of the corner points of the rest pixels in the pixel set as a candidate pose identification pattern corner point in response to the recognized pose identification number being smaller than the pose identification number threshold; and
and matching the plurality of different pose pattern matching templates with the patterns at the corner positions of the candidate pose identification patterns respectively so as to identify the first pose identification.
19. The method of any one of claims 1, 12, 13, further comprising:
determining an arrangement sequence of the plurality of pose identifiers based on at least two of the plurality of pose identifiers; and
and determining three-dimensional coordinates of the plurality of pose identifiers relative to an end coordinate system of the execution arm based on the arrangement sequence of the plurality of pose identifiers.
20. The method according to any one of claims 1, 12, 13,
the plurality of pose markers are disposed on an outer surface of a cylindrical portion of the distal end of the actuator arm.
21. The method of any one of claims 1-5, 7-13, 15-18, further comprising: the actual pose of the end of the actuator arm is determined at a predetermined period to determine a fault condition of the robotic system in real time through a plurality of fault detection cycles.
22. A computer device, comprising:
a memory for storing at least one instruction; and
a processor coupled with the memory and configured to execute the at least one instruction to perform the robotic system fault detection processing method of any one of claims 1-21.
23. A computer-readable storage medium storing at least one instruction that, when executed by a computer, cause the computer to perform the robotic system fault detection processing method of any one of claims 1-21.
24. A robotic system, comprising:
the system comprises an execution arm, wherein the tail end of the execution arm is provided with a plurality of pose marks, and the pose marks comprise different pose mark patterns;
at least one driving device for driving the actuator 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 device is used for acquiring a positioning image of the execution arm; and
control means configured to be connected to the at least one driving means, the at least one driving means sensor, the image capturing device, and to perform the robot system failure detection processing method according to any one of claims 1 to 21.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210141440.3A CN114536401B (en) | 2022-02-16 | 2022-02-16 | Robot system fault detection processing method based on multiple pose identifiers and robot system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210141440.3A CN114536401B (en) | 2022-02-16 | 2022-02-16 | Robot system fault detection processing method based on multiple pose identifiers and robot system |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114536401A CN114536401A (en) | 2022-05-27 |
CN114536401B true CN114536401B (en) | 2024-03-29 |
Family
ID=81674996
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210141440.3A Active CN114536401B (en) | 2022-02-16 | 2022-02-16 | Robot system fault detection processing method based on multiple pose identifiers and robot system |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114536401B (en) |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2009285778A (en) * | 2008-05-29 | 2009-12-10 | Toyota Industries Corp | Posture detecting system of robot hand |
CN105636748A (en) * | 2013-10-17 | 2016-06-01 | 直观外科手术操作公司 | Fault reaction, fault isolation, and graceful degradation in a robotic system |
CN107334531A (en) * | 2017-07-31 | 2017-11-10 | 成都中科博恩思医学机器人有限公司 | A kind of robotic surgery equipment and robotic surgery equipment safety control method |
JP2018144165A (en) * | 2017-03-03 | 2018-09-20 | 株式会社キーエンス | Image processing system, image processing method, image processing program and computer-readable recording medium, and recorded equipment |
CN108765489A (en) * | 2018-05-29 | 2018-11-06 | 中国人民解放军63920部队 | A kind of pose computational methods, system, medium and equipment based on combination target |
CN110455222A (en) * | 2019-07-31 | 2019-11-15 | 中山大学 | A kind of high-precision rotary angle measuring method, device and equipment |
CN111300484A (en) * | 2020-03-13 | 2020-06-19 | 达闼科技成都有限公司 | Method for determining joint positioning error of robot, robot and storage medium |
CN113057735A (en) * | 2021-03-16 | 2021-07-02 | 上海微创医疗机器人(集团)股份有限公司 | Control method of surgical robot system, readable storage medium and robot system |
CN113907885A (en) * | 2020-07-10 | 2022-01-11 | 北京术锐技术有限公司 | Surgical robot and surgical robot withdrawing method |
CN113910219A (en) * | 2020-07-11 | 2022-01-11 | 北京术锐技术有限公司 | Exercise arm system and control method |
CN114536399A (en) * | 2022-01-07 | 2022-05-27 | 中国人民解放军海军军医大学第一附属医院 | Error detection method based on multiple pose identifications and robot system |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE19854011A1 (en) * | 1998-11-12 | 2000-05-25 | Knoll Alois | Device and method for measuring mechanisms and their position |
-
2022
- 2022-02-16 CN CN202210141440.3A patent/CN114536401B/en active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2009285778A (en) * | 2008-05-29 | 2009-12-10 | Toyota Industries Corp | Posture detecting system of robot hand |
CN105636748A (en) * | 2013-10-17 | 2016-06-01 | 直观外科手术操作公司 | Fault reaction, fault isolation, and graceful degradation in a robotic system |
JP2018144165A (en) * | 2017-03-03 | 2018-09-20 | 株式会社キーエンス | Image processing system, image processing method, image processing program and computer-readable recording medium, and recorded equipment |
CN107334531A (en) * | 2017-07-31 | 2017-11-10 | 成都中科博恩思医学机器人有限公司 | A kind of robotic surgery equipment and robotic surgery equipment safety control method |
CN108765489A (en) * | 2018-05-29 | 2018-11-06 | 中国人民解放军63920部队 | A kind of pose computational methods, system, medium and equipment based on combination target |
CN110455222A (en) * | 2019-07-31 | 2019-11-15 | 中山大学 | A kind of high-precision rotary angle measuring method, device and equipment |
CN111300484A (en) * | 2020-03-13 | 2020-06-19 | 达闼科技成都有限公司 | Method for determining joint positioning error of robot, robot and storage medium |
CN113907885A (en) * | 2020-07-10 | 2022-01-11 | 北京术锐技术有限公司 | Surgical robot and surgical robot withdrawing method |
CN113910219A (en) * | 2020-07-11 | 2022-01-11 | 北京术锐技术有限公司 | Exercise arm system and control method |
CN113057735A (en) * | 2021-03-16 | 2021-07-02 | 上海微创医疗机器人(集团)股份有限公司 | Control method of surgical robot system, readable storage medium and robot system |
CN114536399A (en) * | 2022-01-07 | 2022-05-27 | 中国人民解放军海军军医大学第一附属医院 | Error detection method based on multiple pose identifications and robot system |
Also Published As
Publication number | Publication date |
---|---|
CN114536401A (en) | 2022-05-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114347037B (en) | Robot system fault detection processing method based on composite identification and robot system | |
US10551821B2 (en) | Robot, robot control apparatus and robot system | |
US9089971B2 (en) | Information processing apparatus, control method thereof and storage medium | |
JP5365379B2 (en) | Robot system and robot system calibration method | |
US11443501B2 (en) | Robotic surgical safety via video processing | |
CN114536399B (en) | Error detection method based on multiple pose identifications and robot system | |
CN114536292A (en) | Error detection method based on composite identification and robot system | |
CN114523471B (en) | Error detection method based on association identification and robot system | |
Burrell et al. | Towards a cooperative robotic system for autonomous pipe cutting in nuclear decommissioning | |
Lambrecht | Robust few-shot pose estimation of articulated robots using monocular cameras and deep-learning-based keypoint detection | |
CN114536402B (en) | Robot system fault detection processing method based on association identification and robot system | |
CN114536401B (en) | Robot system fault detection processing method based on multiple pose identifiers and robot system | |
CN115731289A (en) | Method for determining pose of object and surgical robot system | |
CN115946105A (en) | Control method of operation arm and surgical robot system | |
Cofield et al. | A humanoid robot object perception approach using depth images | |
US20240185432A1 (en) | System, method, and apparatus for tracking a tool via a digital surgical microscope | |
CN116492064A (en) | Master-slave motion control method based on pose identification and surgical robot system | |
CN114536329B (en) | Method for determining external stress of deformable mechanical arm based on composite identification and robot system | |
CN116468646A (en) | Execution arm detection method based on composite identification and robot system | |
CN116468647A (en) | Execution arm detection method based on multiple pose identifiers and robot system | |
CN116468648A (en) | Execution arm detection method based on association identification and robot system | |
CN116459018A (en) | Operation arm anti-collision control method based on composite identification and operation robot system | |
CN116459019A (en) | Pose identification-based control method for preventing collision of operation arm and surgical robot system | |
CN116460837A (en) | Operation arm anti-collision control method based on association identification and operation robot system | |
CN115731290A (en) | Method for determining pose of object and surgical robot system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
CB02 | Change of applicant information | ||
CB02 | Change of applicant information |
Address after: 100005 Wangfujing, Beijing Dongcheng District Wangfujing Shuai Fu Garden No. 1 Applicant after: PEKING UNION MEDICAL COLLEGE Hospital Applicant after: Beijing Shurui Robot Co.,Ltd. Address before: 100005 Wangfujing, Beijing Dongcheng District Wangfujing Shuai Fu Garden No. 1 Applicant before: PEKING UNION MEDICAL COLLEGE Hospital Applicant before: BEIJING SURGERII TECHNOLOGY Co.,Ltd. |
|
GR01 | Patent grant | ||
GR01 | Patent grant |