A geometric body center pose determination method, device, equipment, medium and product

Abstract

This invention discloses a method, apparatus, device, medium, and product for determining the center pose of a geometric object, relating to the field of robotic arm technology. The method involves acquiring the visual detection results of a target geometric object in the current frame, issued by a vision system; traversing the visual detection results to extract at least one target QR code and its corresponding position and pose information; calculating the position and pose information based on a rotation mapping table and a pose offset vector list to obtain candidate center pose information corresponding to the target QR code; and performing end-effector localization calculations on the candidate center pose information based on standard pose information to determine the target center pose information. This technical solution, by setting a rotation mapping table and a pose offset vector list to estimate the center pose information of the geometric object, ensures that the center pose information of each face of the geometric object is under the same reference direction, solving the problem of pose information estimation changes when switching different faces of the existing geometric object.

Description

Technical Field

[0001] This invention relates to the field of robotic arm technology, and in particular to a method, apparatus, device, medium, and product for determining the center pose of a geometric body. Background Technology

[0002] When performing tracking, docking, or grasping operations, a common solution for robotic arm vision systems is to use cooperative targets for positioning and pose measurement. This involves designing a geometric shape of known size and shape on the target beforehand, and then using the vision system to obtain the target's pose information by observing the shape. The pose is then fed back to the robotic arm's end effector.

[0003] However, when the target is a cube and the recognition features of each station are located on different surfaces, the origin and axis of the coordinate system on which the output pose is based will change with the observed surface. This output jump caused by the change of the observed plane itself will disrupt the continuity of the target value of the visual servo control system. Furthermore, when the object's posture changes, causing the camera to observe different surfaces, such as changing from the front to the top, the image content will change fundamentally. If the positioning is strictly based on a single-view template bound to the preset pose, the template matching conditions no longer exist, causing the pose estimation to fail and the system to be unable to complete the positioning.

[0004] Therefore, there is an urgent need for a method to determine the center pose of a geometric object, which can ensure that the center pose is in the same reference direction regardless of which face of the geometric object is currently visible. Summary of the Invention

[0005] This invention provides a method, apparatus, device, medium, and product for determining the center pose of a geometric body, which solves the problem that the pose information estimation changes when switching different faces of a geometric body. By setting a rotation mapping table and a posture offset vector list, the center pose information of the geometric body is estimated, ensuring that the center pose information of each face of the geometric body is under the same reference direction.

[0006] According to one aspect of the present invention, a method for determining the pose of the center of a geometric body is provided, comprising: Obtain the visual detection results of the target geometry in the current frame, as sent by the vision system; The visual detection results are traversed to extract at least one target QR code and the position and pose information corresponding to the target QR code; the position and pose information includes position coordinates and target pose quaternions. The position and attitude information is calculated based on the rotation mapping table and the attitude offset vector list to obtain the candidate center pose information corresponding to the target QR code; Based on the standard pose information, the candidate center pose information is used to perform end-effector localization calculation to determine the target center pose information.

[0007] According to another aspect of the present invention, a geometric center pose determination device is provided, comprising: The acquisition module is used to acquire the visual detection results of the target geometry in the current frame issued by the vision system; The traversal module is used to traverse the visual detection results and extract at least one target QR code and the position and pose information corresponding to the target QR code; the position and pose information includes position coordinates and target pose quaternions. The calculation module is used to calculate the position and attitude information based on the rotation mapping table and the attitude offset vector list to obtain the candidate center pose information corresponding to the target QR code; The determination module is used to perform end-effector localization calculation on the candidate center pose information based on standard pose information to determine the target center pose information.

[0008] According to another aspect of the present invention, an electronic device is provided, the electronic device comprising: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores a computer program that can be executed by the at least one processor, the computer program being executed by the at least one processor to enable the at least one processor to perform the geometric center pose determination method according to any embodiment of the present invention.

[0009] According to another aspect of the present invention, a computer-readable storage medium is provided, the computer-readable storage medium storing computer instructions for causing a processor to execute and implement the geometric center pose determination method according to any embodiment of the present invention.

[0010] According to another aspect of the present invention, a computer program product is provided, the computer program product comprising a computer program that, when executed by a processor, implements the geometric center pose determination method according to any embodiment of the present invention.

[0011] The technical solution of this invention estimates the center pose information of a geometry by setting a rotation mapping table and a pose offset vector list, ensuring that the center pose information of each face of the geometry is in the same reference direction, thus solving the problem that the pose information estimation changes when switching different faces of the existing geometry.

[0012] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0013] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0014] Figure 1 This is a flowchart of a method for determining the center pose of a geometric object according to an embodiment of the present invention; Figure 2 This is a flowchart of a method for determining candidate center pose information according to an embodiment of the present invention; Figure 3 This is a flowchart of a method for determining the center pose of a geometric object according to an embodiment of the present invention; Figure 4 This is a schematic diagram of a geometric body center pose determination device according to an embodiment of the present invention; Figure 5 This is a schematic diagram of the structure of an electronic device that implements the geometric center pose determination method of the present invention. Detailed Implementation

[0015] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0016] It should be noted that the terms "comprising" and "having" and any variations thereof in the specification, claims and accompanying drawings of this invention are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such process, method, product or device.

[0017] Furthermore, it should be noted that the information collected in the technical solution of this invention is information and data authorized by the user or fully authorized by all parties, and the collection, storage, use, processing, transmission, provision, disclosure and application of related data all comply with the relevant laws, regulations and standards of relevant countries and regions, necessary confidentiality measures have been taken, and public order and good morals are not violated. Corresponding operation entry points are provided for users to choose to authorize or refuse.

[0018] Figure 1 This invention provides a flowchart of a method for determining the center pose of a geometric object. This invention is applicable to estimating the center pose of a multi-faceted QR code geometry, particularly when there are multiple visible faces. The method can be executed by a geometric object center pose determination device, which can be implemented in hardware and / or software. This device can be configured in a server, such as a computing unit integrated into a humanoid robot. Figure 1 As shown, the method includes: S110. Obtain the visual detection results of the target geometry in the current frame issued by the vision system.

[0019] The vision system receives video streams uploaded by sensors and can be an RCO (Robot Operating System) visual servo. The target geometry is a geometry with multiple faces, preferably a cube. Each face of the cube, such as the front, left, right, top, and back, is assigned a unique QR code identifier, and the QR code identifiers are arranged in a fixed order to form a QR code identifier list. The current frame is the image frame in the video stream where visual detection is being performed, including the currently visible faces of the geometry. The visual detection result is the QR code detection result of the visible faces of the target geometry.

[0020] Specifically, the visual detection results of the target geometry in the current frame obtained by the vision system can be represented as a visible QR code identifier of the target geometry in the current frame.

[0021] S120. Traverse the visual detection results and extract at least one target QR code and the corresponding position and pose information of the target QR code.

[0022] The position and attitude information includes position coordinates and attitude quaternions; the target QR code is the QR code of the visible face of the geometry in the current frame, and there can be at least one; the target QR code carries a target QR code identifier for each face, which can be the index number corresponding to each face of the geometry. For example, if the geometry is a cube, its front, left, right, top, and back faces are assigned corresponding index numbers as QR code identifiers. For example, the front face is the reference face, and its index number is 0; the left face has an index number of 1; the right face has an index number of 2; the top face has an index number of 3; and the back face has an index number of 4. These index numbers are then combined into a QR code identifier list; the position coordinates in the position and attitude information are the position coordinates of the target QR code in the world coordinate system; the target attitude quaternion in the position and attitude information is the attitude description of the target QR code, described in the structure of a quaternion.

[0023] Specifically, the visual detection results of the current frame are traversed to obtain all detected target QR codes in the current frame, along with the position coordinates and target pose quaternion of each target QR code in the world coordinate system. If the target QR code identifier belongs to the above tag_ids, it is determined that a face of the geometry has been observed, and the current_tag_id, position p (x, y, z), and target pose quaternion tag_quat of the face containing the target QR code are recorded. .

[0024] S130. The position and attitude information is calculated based on the rotation mapping table and the attitude offset vector list to obtain the candidate center pose information corresponding to the target QR code.

[0025] The rotation mapping table represents the mapping relationship between the QR code identifier of each face of the geometry and the rotation quaternion from the face's own coordinate system to the reference face's coordinate system, and can be tagged as tag_rotations. It can include, for example, setting the reference face as the front face of the geometry; for instance, if the reference face has no rotation, then its corresponding rotation quaternion does not exist; if the left face of the geometry needs to be rotated 90° around the y-axis to the reference face, then its corresponding rotation quaternion is represented as [0, ,0, The right side of the geometric solid needs to be rotated -90° around the y-axis to the reference plane. The corresponding rotation quaternion is represented as [0, ...]. ,0, The right side of the geometric solid needs to be rotated -90° around the x-axis to the reference plane. The corresponding rotation quaternion is represented as […]. ,0,0, The back face of the geometry needs to be rotated 180° around the y-axis to the reference plane, and its corresponding rotation quaternion is represented as [0,0,1,0]. The attitude offset vector list is the correspondence between the QR code identifier of each face of the geometry and the offset vector from that face to the geometric center of the geometry, which can be marked as tag_offset_in_tag_frame. The offset vector is the offset vector of different QR code planes of the cube to the geometric center of the cube in its own coordinate system. The attitude offset vector can be determined according to the size of the geometry and the position coordinates of the target QR code in the world coordinate system. The rotation mapping table and the attitude offset vector list can be updated in real time according to the actual situation. The candidate center pose information is the position coordinate information and attitude quaternion of each QR code plane converted into position coordinate information and attitude information in the same reference direction.

[0026] Specifically, the target rotation quaternion corresponding to the target QR code is found based on the mapping relationship between the QR code identifier and the rotation quaternion in the rotation mapping table. The attitude offset vector corresponding to the target QR code is found based on the correspondence between the QR code identifier and the attitude offset vector in the attitude offset vector list. The position coordinates and target attitude quaternion in the position attitude information are calculated based on the rotation quaternion and the attitude offset vector, respectively, to obtain the candidate center pose information of the target QR code under the same reference direction.

[0027] Optional, such as Figure 2 The method for determining candidate center pose information, as shown, calculates position and pose information based on a rotation mapping table and a list of pose offset vectors to obtain candidate center pose information corresponding to the target QR code, including: S131. Parse the target QR code to obtain the target QR code identifier.

[0028] Among them, the target QR code identifier is used to uniquely identify the QR code on each face of the geometric object.

[0029] Specifically, the target QR code obtained from the visual inspection results is parsed to obtain the corresponding target QR code identifier, which can be an index number.

[0030] S132. Determine the center attitude quaternion based on the target QR code identifier, rotation mapping table, and target attitude quaternion.

[0031] Among them, the center attitude quaternion is the attitude of the center of the surface where the target QR code is located under the reference plane.

[0032] Specifically, based on the target QR code identifier, the rotation quaternion corresponding to the face where the target QR code identifier is located is found from the rotation mapping table. The target attitude quaternion and the attitude corresponding to the rotation quaternion are calculated by quaternion multiplication and used as the center attitude quaternion.

[0033] Optionally, the center attitude quaternion is determined based on the target QR code identifier, the rotation mapping table, and the target attitude quaternion, including: If the target QR code is identified as a reference surface identifier, then the target attitude quaternion corresponding to the target QR code is used as the center attitude quaternion. If the target QR code identifier is not the reference face identifier, then the rotation quaternion corresponding to the target QR code identifier is determined from the rotation mapping table based on the target QR code identifier; The center attitude quaternion is determined based on the rotation quaternion and the target attitude quaternion.

[0034] Among them, the reference surface identifier is a pre-defined QR code identifier used to express the orientation of each surface of the geometry as the reference direction.

[0035] Specifically, it is determined whether the target QR code identifier is a reference surface identifier. If the currently visible target QR code identifier is a reference surface identifier, the target attitude quaternion corresponding to the target QR code is directly used as the center attitude quaternion. If the target QR code identifier is not a reference surface identifier, the rotation quaternion corresponding to the target QR code identifier is determined from the rotation mapping table based on the target QR code identifier. The center attitude quaternion is calculated by quaternion multiplication based on the rotation quaternion and the target attitude quaternion. For example, let the target attitude quaternion of the currently detected target QR code be tag_quat. If the surface where the target QR code is located is a reference surface, the center attitude quaternion center_quat is equal to the target attitude quaternion tag_quat. If the surface where the target QR code is located is not a reference surface, the rotation quaternion tag_to_ref_rotation from the target QR code identifier is found from the rotation mapping table tag_rotations, and the center attitude quaternion center_quat = tag_quat × tag_to_ref_rotation is calculated by quaternion multiplication.

[0036] Understandably, by using the rotation quaternions in a predefined rotation mapping table and the rotation relationships between the faces of the geometry, and based on the visible faces of the geometry detected in the current frame from the visual detection results, the corresponding rotation quaternions are found. Based on the rotation quaternions, the rotation calculation of the target pose quaternions is performed, thereby uniformly transforming the pose of any visible face of the geometry to the reference plane coordinate system. By assigning corresponding QR code identifiers to each face of the geometry and predefining their fixed rotations to the reference plane, the pose of any visible face is uniformly transformed to an invariant reference coordinate system, ensuring the consistency of the output pose and avoiding target jumps. It ensures that no matter which face is currently visible, the output pose is expressed in the same reference direction, avoiding target jumps caused by changes in the visible face, and facilitating stable convergence of visual servoing or trajectory planning.

[0037] S133. Determine the attitude offset vector corresponding to the target QR code identifier from the attitude offset vector list based on the target QR code identifier.

[0038] Specifically, the attitude offset vector corresponding to the target QR code identifier is determined from the offset vector list. For example, the center offset vector corresponding to the target QR code identifier in its own coordinate system on the surface of the QR code is found from the offset vector list as t_offset_tag = .

[0039] S134. Determine the target offset vector based on the target attitude quaternion and the attitude offset vector.

[0040] The target offset vector is the center offset vector of the surface where the QR code is located, transformed into the world coordinate system.

[0041] Specifically, the target pose quaternion is transformed to obtain its corresponding pose rotation matrix. The target offset vector is obtained by the dot product of the pose rotation matrix and the pose offset vector. For example, the position of the currently detected QR code identifier in the world coordinate system is p= The attitude rotation matrix R_tag can be derived from the target attitude quaternion tag_quat. The offset vector is then transformed to the world coordinate system, and its target offset vector t_offset_world = R_tag⋅t_offset_tag.

[0042] S135. Determine the center position coordinates of the target geometry based on the target offset vector and position coordinates.

[0043] The center position coordinates represent the position of the face containing the QR code identifier of the geometric object in the world coordinate system.

[0044] Specifically, the center position coordinates of the target cube are obtained by summing the target offset vector and the position coordinates. For example, the position of the geometric center in the world coordinate system is: center_position = p + t_offset_world.

[0045] S136. Determine candidate center pose information based on center position coordinates and center pose quaternions.

[0046] Specifically, the center pose of the face where the target QR code is located in the world coordinate system is directly described based on the three-dimensional position of the center position coordinates and the center pose quaternion; the candidate center pose information of the face where the target QR code is located is directly output, such as center position coordinates represented as center_position; center pose quaternion represented as center_quat.

[0047] Understandably, by predefining a rotation mapping table and a list of pose offset vectors, the center poses of multiple faces of the geometry are expressed in the same reference direction, effectively avoiding target jumps caused by visible face switching. This facilitates stable convergence of visual servoing or trajectory planning. Furthermore, it effectively utilizes the strong geometric prior that "the center has a fixed offset relative to each face." Through a single coordinate transformation, such as adding the rotation offset vector to the detection position, the center position can be analytically calculated. This eliminates the need for multiple faces to be visible simultaneously, as well as multi-frame filtering or complex optimization, thus combining strong robustness with high real-time performance.

[0048] S140. Based on the standard pose information, perform end-effector localization calculation on the candidate center pose information to determine the target center pose information.

[0049] The standard pose information consists of the predefined center coordinates and orientation information of each face of the geometry; the end-effector localization calculation is a localization adjustment, which adjusts the candidate center pose information based on the standard pose information; the target center pose information is the updated center pose information corresponding to the currently visible face of the geometry in the current frame, which can be output. It is used to convert the center pose information into robot joint angle commands through inverse kinematics and send them to the robot's robotic arm joint driver for execution, thereby completing a servo adjustment.

[0050] Specifically, based on the standard pose information, the robot determines whether the candidate center pose information of the visible face of the geometry in the current frame is in place. If it reaches the end position under the standard pose, it means that the robot arm can perform grasping of the geometry without adjustment, and the candidate center pose information is used as the target center pose information. If it does not reach the end position under the standard pose, it means that the robot arm needs to be adjusted to perform accurate grasping of the geometry, and the candidate center pose information is adjusted according to the standard pose information. The updated candidate center pose information is used as the target center pose information so that the robot arm can perform accurate grasping of the geometry based on the target center pose information.

[0051] This invention addresses the problem of pose estimation changes when switching between different faces of a geometric object. The invention involves acquiring the visual detection results of a target geometry within the current frame from a vision system; traversing the visual detection results to extract at least one target QR code and its corresponding position and pose information; calculating the position and pose information based on a rotation mapping table and a pose offset vector list to obtain candidate center pose information corresponding to the target QR code; and performing end-effector localization calculations based on standard pose information to determine the target center pose information. This technical solution, by setting a rotation mapping table and a pose offset vector list to estimate the center pose information of the geometry, ensures that the center pose information of each face of the geometry is in the same reference direction, thus solving the problem of pose estimation changes when switching between different faces of the existing geometry.

[0052] Figure 3 This is a flowchart of a method for determining the center pose of a geometric object according to an embodiment of the present invention. The embodiments of the present invention supplement the method for determining the target center pose information based on the above embodiments. It should be noted that for parts not described in detail in the embodiments of the present invention, please refer to the relevant descriptions in other embodiments. For example... Figure 3 As shown, the method includes: S210. Obtain the visual detection results of the target geometry in the current frame issued by the vision system.

[0053] S220. Traverse the visual detection results and extract at least one target QR code and the position and pose information corresponding to the target QR code.

[0054] The position and attitude information includes position coordinates and attitude quaternions.

[0055] S230. The position and attitude information is calculated based on the rotation mapping table and the attitude offset vector list to obtain the candidate center pose information corresponding to the target QR code.

[0056] S240. Compare the standard pose information with the candidate center pose information. If the standard pose information and the candidate center pose information are inconsistent, determine the pose error based on the standard pose information and the candidate center pose information.

[0057] Among them, the pose error is the pose deviation between the standard pose information and the candidate center pose information, which is used to determine whether the geometric pose calculated in the current frame can complete the capture.

[0058] Specifically, the pose error corresponding to the current visible surface of the geometry in the current frame is determined based on the standard pose information and the candidate center pose information.

[0059] In an optional embodiment of the present invention, if the standard pose information and the candidate center pose information match, it indicates that the current geometry is located at a position where the robot's end effector can accurately execute. In this case, no pose adjustment is required, and the candidate center pose information is directly used as the target center pose information so that the robot can control the robotic arm to complete servoing based on the target center pose information.

[0060] S250. The pose adjustment amount of the current frame is calculated based on the pose error.

[0061] Among them, the pose adjustment amount is the pose error on each coordinate axis in Cartesian space.

[0062] Specifically, after obtaining the pose error, its error on each coordinate axis in Cartesian space is calculated; for example, the standard pose information is... The candidate center pose information is represented as follows: The error is the difference between the standard pose information and the candidate center pose information, expressed as: The pose adjustment amount in Cartesian space can be calculated based on the current pose error, and is expressed as follows: .

[0063] S260. The pose adjustment amount of the current frame is superimposed with the center pose information of the previous frame to obtain the target center pose information of the current frame.

[0064] Specifically, the pose adjustment amount of the current frame is superimposed with the center pose information of the previous frame to obtain the target center pose information of the current frame. Then, through inverse kinematics, the target center pose information is converted into robot joint angle commands and sent to the robot arm joint driver for execution, thereby completing a servo adjustment.

[0065] In an optional embodiment of the present invention, the above-described embodiment is integrated as a lightweight module into a complete two-stage PID vision servo controller. This allows for the continuous acquisition of the real-time pose of the cube center fixed on the robot's end effector during the servo control loop. This pose serves as feedback for the control system, ensuring that the error vector between the feedback value and the standard pose information is used as the basis for subsequent adjustments. When the error is small, a first-order low-pass filter can be activated to smooth the feedback data and suppress noise. For example, when the absolute value of the error on each axis is less than the corresponding axis's filter activation threshold, the system is determined to have entered the fine alignment stage. At this point, the filter is activated to smooth the subsequent feedback pose, thereby suppressing the impact of high-frequency measurement noise on control stability.

[0066] Understandably, by employing a two-stage PID controller, the real-time pose output by the aforementioned cube center pose estimation algorithm is used as the feedback value. The control mode is automatically switched according to the magnitude of the error. The first stage performs coarse adjustment when the multi-axis error is large. When the error enters the fine threshold, it automatically switches to the second stage to focus on single-axis fine adjustment, avoid overshoot, and ensure smooth convergence.

[0067] In an optional embodiment of the present invention, when the visible faces of the geometry in the current frame are not unique, the QR code identifier of the i-th detected face is denoted as tag_id_i, and its position in the world coordinate system is... The attitude quaternion is Then its corresponding center attitude quaternion is represented as: The center position coordinates are represented as Thus, a set of pose center estimates for the same physical center but based on different observation planes is obtained: P={( , ),( , ),…,( , )}, where N is the number of detected faces.

[0068] Understandably, by setting standard pose information to precisely fine-tune the pose information of the candidate center, the problem that the origin and axis of the coordinate system on which the output pose is based will change with the different observed surfaces when the target is a cube and the recognition features of each station are located on different surfaces can be effectively avoided, thus enabling the robot arm to accurately grasp the target geometry.

[0069] Optionally, after superimposing the pose adjustment amount of the current frame with the center pose information of the previous frame to obtain the target center pose information of the current frame, the method further includes: If there are at least two target center pose information, then the mean position coordinates of at least one center position are calculated to obtain the mean position coordinates; The position error is determined based on the mean position coordinates and at least one center position coordinate. The mean value of at least one center attitude quaternion is calculated to obtain the average attitude quaternion. The sum of squared angle differences is calculated based on the average attitude quaternion and at least one central attitude quaternion. If the sum of squares of position error and angle difference is greater than the error threshold, then the rotation quaternion and attitude offset vector in the rotation mapping table and attitude offset vector list are optimized based on at least one center position coordinate and at least one center attitude quaternion, respectively.

[0070] Wherein, the mean position coordinate is the mean of at least one center position coordinate; the position error is the difference between each center position coordinate and the mean position coordinate; the average attitude quaternion is the mean of at least one center attitude quaternion; the sum of squared angle differences is the angle difference between each center attitude quaternion and the average attitude quaternion; the error threshold is used to determine whether to perform predefined parameter optimization; if the error threshold is exceeded, it indicates that the parameters in the current rotation map table and attitude offset vector need to be corrected and updated, and do not conform to the current actual geometric position estimation; if the error threshold is not exceeded, it indicates that the parameters in the current rotation map table and attitude offset vector do not need to be corrected and updated, and conform to the current actual geometric position estimation.

[0071] Specifically, if there are at least two target center pose information, the parameters in the predefined mapping table and offset vector list can be optimized and estimated based on the center pose of the geometric facets. The mean coordinates of at least one center position are calculated to obtain the mean position coordinates. The position error is determined based on the difference between each center position coordinate and the mean position coordinates. The mean quaternion of at least one center attitude is calculated to obtain the average attitude quaternion. The sum of squared angle differences between each center attitude quaternion and the average attitude quaternion is calculated. If the sum of squared angle differences does not exceed the error threshold, it indicates that the parameters in the current rotation mapping table and attitude offset vector do not need to be corrected or updated, and are consistent with the current actual geometric position estimation. If both the position error and the sum of squared angle differences are greater than the error threshold, it indicates that the parameters in the current rotation mapping table and attitude offset vector need to be corrected and updated, and are inconsistent with the current actual geometric position estimation. Then, the rotation quaternions and attitude offset vectors in the rotation mapping table and attitude offset vector list are optimized based on at least one center position coordinate and at least one center attitude quaternion, respectively.

[0072] Understandably, by setting an error threshold and using selectable multi-face simultaneous observation data to optimize and correct predefined rotation and offset parameters online, the problem of inconsistent estimation results for different faces caused by physical errors can be effectively suppressed, ensuring the consistency of output pose. This can be executed periodically during operation, continuously adapting to environmental changes and fundamentally eliminating end-control jumps caused by face switching.

[0073] Optionally, the rotation quaternions and attitude offset vectors in the rotation mapping table and attitude offset vector list are optimized based on at least one center position coordinate and at least one center attitude quaternion, respectively, including: The parameter vector is defined based on the rotation quaternion in the rotation map table and the attitude offset vector in the attitude offset vector list; Construct a least-squares optimization function based on the parameter vector and at least one target center pose information; If there are two center position coordinates, then determine the attitude offset vector corresponding to the center position coordinates; Geometric correction of the attitude offset vector is performed based on the center position coordinates and the least squares optimization function, respectively. If there are at least three center position coordinates, then determine the number of position and attitude information and the initial parameters of the least squares optimization function; The residual vector is determined based on different center position coordinates; The initial parameters of the least squares optimization function are iteratively optimized based on the amount of position and attitude information and the residual vector to obtain the optimized parameters.

[0074] The parameter vector consists of rotation quaternions in a predefined rotation mapping table and attitude offset vectors in an attitude offset vector list; the initial parameters are the parameter vectors in the least squares optimization function, i.e., rotation quaternions in a predefined rotation mapping table and attitude offset vectors in an attitude offset vector list; the residual vector is the difference between any two different center position coordinates; and the optimization parameters are the optimization parameters of the least squares optimization function, used to update the rotation quaternions in the predefined rotation mapping table and the attitude offset vectors in the attitude offset vector list.

[0075] Specifically, a parameter vector is defined based on the rotation quaternions in the rotation mapping table and the attitude offset vectors in the attitude offset vector list. A least-squares optimization function is constructed based on the parameter vectors and at least one target center pose information. If two center position coordinates exist, it indicates that their corresponding center position coordinates are coincident, and no optimization correction of the rotation quaternions is needed; therefore, the attitude offset vector corresponding to the center position coordinates is determined. Geometric corrections are then performed on the attitude offset vector based on the center position coordinates and the least-squares optimization function. For example, if the center position coordinates are on the left, reference plane, and top, the attitude offset vector corresponding to the left-side center position coordinates is optimized using the least-squares optimization function to obtain the updated attitude offset vector. The attitude offset vector is updated to the attitude offset vector list so that it can be directly calculated in the next center pose estimation. If there are at least three center position coordinates, the number of position and attitude information and the initial parameters of the least squares optimization function are determined. The residual vector is determined by searching the center position between different face combinations corresponding to the center position coordinates. The initial parameters of the least squares optimization function are iteratively optimized according to the number of position and attitude information and the residual vector until the initial parameters meet the set value or the number of iterations reaches the preset number of position and attitude information. Then, the optimized parameters of the least squares optimization function are used as update parameters to update and replace the rotation quaternions in the rotation mapping table and the attitude offset vector in the attitude offset vector list.

[0076] For example, when obtaining the target center pose information corresponding to the current frame, for the pose center estimation set: P={( , ),( , ),…,( , First, calculate the mean. The formula for calculating the mean is as follows: Where N is a positive integer, representing the observation surface observed by the geometric object; Indicates the coordinates of the mean position; Based on mean location coordinates The position error is quantized using the coordinates of each center position to obtain the position error. The quantification formula is as follows: ; The rotations in the rotation map (parameterized as Euler angles) Offset vectors in the attitude offset vector list To optimize variables, define a parameter vector. Where M is the total number of faces of the geometry; a least-squares optimization function is constructed based on the parameter vector and at least one target center pose information: ; in, These are weighting coefficients, based on detection confidence. , Assignment ∈[0,1]; This represents the center position calculated based on the observation data of the i-th surface under the parameter Ω.

[0077] If there are two center position coordinates and When, its corresponding rotation matrix is and Then, the attitude offset vector can be optimized based on the center position coordinates and the least squares optimization function to find a value t such that the equation optimally considers the detection confidence of the two surfaces in the least squares sense. and The optimized attitude offset vector is then obtained as follows: The optimized formula is as follows:

[0078] If there are at least three center position coordinates, the initial parameters in the least squares optimization function can be set. Damping factor λ, maximum number of iterations Convergence threshold For each iteration step k=0,1,… -1; Calculate the residual vector The calculation formula is as follows:

[0079] Where (m=1,2,…,C(N,2)) corresponds to different face combinations (i,j), and C(N,2)=(N(N-1)) / 2 is the number of combinations.

[0080] Iterative parameter updates and optimizations are performed based on the residual vector and initial parameters, resulting in improved parameters. ;like or k= -1, stop iteration, output the optimal solution. : The new parameters obtained through optimization Update the rotation map and attitude offset vector list in the system to ensure that subsequent single-face pose estimations will use this optimized parameter, thereby guaranteeing the consistency of the output pose.

[0081] Understandably, by using optional multi-face simultaneous observation data, predefined rotation and offset parameters are optimized and corrected online to ensure that subsequent single-face pose estimation will use these optimized parameters, thereby guaranteeing the consistency of the output pose. This correction process can be actively triggered during the system initialization phase to obtain accurate initial values, or it can be executed periodically during operation to continuously adapt to environmental changes, fundamentally eliminating end-point control jumps caused by face switching. The optimization algorithm is encapsulated through functions and seamlessly integrated with a mature two-stage PID visual servo framework. Only the rotation mapping table and attitude offset vector list corresponding to the QR code identifier list in the configuration file need to be modified to adapt to new objects, making it highly practical for engineering applications.

[0082] This invention acquires multi-faceted observation data of a geometric object. If the mean error and the sum of squares of multiple angles of the obtained target center pose are greater than the error threshold, the predefined rotation and offset parameters are optimized and corrected online. This can effectively suppress the problem of inconsistent estimation results of different faces caused by physical errors, and ensure the long-term stability and accuracy of the output pose from the source.

[0083] Figure 4 This invention provides a schematic diagram of a geometric center pose determination device. This invention is applicable to estimating the center pose of multi-faceted QR code geometries, particularly when there are multiple visible faces. The geometric center pose determination device can be implemented in hardware and / or software and can be configured in a server, such as a computing unit integrated into a humanoid robot. Figure 4 As shown, the geometric center pose determination device 300 includes a visual detection result acquisition module 310, a pose extraction module 320, a pose calculation module 330, and a pose determination module 340. The visual detection result acquisition module 310 is used to acquire the visual detection result of the target geometry in the current frame issued by the vision system. The pose extraction module 320 is used to traverse the visual detection results and extract at least one target QR code and the position and pose information corresponding to the target QR code; the position and pose information includes position coordinates and target pose quaternions. The pose calculation module 330 is used to calculate the position and pose information based on the rotation mapping table and the pose offset vector list to obtain the candidate center pose information corresponding to the target QR code. The pose determination module 340 is used to perform end-effector localization calculation on candidate center pose information based on standard pose information to determine the target center pose information.

[0084] This invention addresses the problem of pose estimation changes when switching between different faces of a geometric object. The invention involves acquiring the visual detection results of a target geometry within the current frame from a vision system; traversing the visual detection results to extract at least one target QR code and its corresponding position and pose information; calculating the position and pose information based on a rotation mapping table and a pose offset vector list to obtain candidate center pose information corresponding to the target QR code; and performing end-effector localization calculations based on standard pose information to determine the target center pose information. This technical solution, by setting a rotation mapping table and a pose offset vector list to estimate the center pose information of the geometry, ensures that the center pose information of each face of the geometry is in the same reference direction, thus solving the problem of pose estimation changes when switching between different faces of the existing geometry.

[0085] Optionally, the pose calculation module 330 is also used to parse the target QR code to obtain the target QR code identifier; The center attitude quaternion is determined based on the target QR code identifier, rotation mapping table, and target attitude quaternion. Determine the attitude offset vector corresponding to the target QR code identifier from the attitude offset vector list based on the target QR code identifier; The target offset vector is determined based on the target attitude quaternion and the attitude offset vector. Determine the center coordinates of the target cube based on the target offset vector and position coordinates; Candidate center pose information is determined based on the center position coordinates and center pose quaternions.

[0086] Optionally, the pose calculation module 330 is also used to take the target pose quaternion corresponding to the target QR code as the center pose quaternion if the target QR code identifier is a reference surface identifier. If the target QR code identifier is not the reference face identifier, then the rotation quaternion corresponding to the target QR code identifier is determined from the rotation mapping table based on the target QR code identifier; The center attitude quaternion is determined based on the rotation quaternion and the target attitude quaternion.

[0087] Optionally, the pose determination module 340 is also used to compare the standard pose information and the candidate center pose information. If the standard pose information and the candidate center pose information are inconsistent, the pose error is determined based on the standard pose information and the candidate center pose information. The pose adjustment amount for the current frame is calculated based on the pose error. The pose adjustment of the current frame is superimposed with the center pose information of the previous frame to obtain the target center pose information of the current frame.

[0088] Optionally, the geometry center pose determination device 300 further includes an optimization module, which is used to calculate the mean value of at least one center position coordinate if there are at least two target center pose information to obtain the mean position coordinate. The position error is determined based on the mean position coordinates and at least one center position coordinate. The mean value of at least one center attitude quaternion is calculated to obtain the average attitude quaternion. The sum of squared angle differences is calculated based on the average attitude quaternion and at least one central attitude quaternion. If the sum of squares of position error and angle difference is greater than the error threshold, then the rotation quaternion and attitude offset vector in the rotation mapping table and attitude offset vector list are optimized based on at least one center position coordinate and at least one center attitude quaternion, respectively.

[0089] Optionally, the optimization module is also used to define parameter vectors based on rotation quaternions in the rotation map table and attitude offset vectors in the attitude offset vector list; Construct a least-squares optimization function based on the parameter vector and at least one target center pose information; If there are two center position coordinates, then determine the attitude offset vector corresponding to the center position coordinates; Geometric correction of the attitude offset vector is performed based on the center position coordinates and the least squares optimization function, respectively. If there are at least three center position coordinates, then determine the number of position and attitude information and the initial parameters of the least squares optimization function; The residual vector is determined based on different center position coordinates; The initial parameters of the least squares optimization function are iteratively optimized based on the amount of position and attitude information and the residual vector to obtain the optimized parameters.

[0090] The geometric center pose determination device provided in the embodiments of the present invention can execute the geometric center pose determination method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the method.

[0091] According to embodiments of the present invention, the present invention also provides an electronic device, a readable storage medium, and a computer program product.

[0092] Figure 5A schematic diagram of an electronic device 10 that can be used to implement embodiments of the present invention is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital assistants, cellular phones, smartphones, wearable devices (e.g., helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0093] like Figure 5 As shown, the electronic device 10 includes at least one processor 11 and a memory, such as a read-only memory (ROM) 12 or a random access memory (RAM) 13, communicatively connected to the at least one processor 11. The memory stores computer programs executable by the at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the ROM 12 or loaded from storage unit 18 into the RAM 13. The RAM 13 can also store various programs and data required for the operation of the electronic device 10. The processor 11, ROM 12, and RAM 13 are interconnected via a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0094] Multiple components in electronic device 10 are connected to I / O interface 15, including: input unit 16, such as keyboard, mouse, etc.; output unit 17, such as various types of displays, speakers, etc.; storage unit 18, such as disk, optical disk, etc.; and communication unit 19, such as network card, modem, wireless transceiver, etc. Communication unit 19 allows electronic device 10 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0095] Processor 11 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, digital signal processors (DSPs), and any suitable processor, controller, microcontroller, etc. Processor 11 performs the various methods and processes described above, such as the geometry center pose determination method.

[0096] In some embodiments, the geometry center pose determination method may be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program may be loaded and / or mounted on electronic device 10 via ROM 12 and / or communication unit 19. When the computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the transaction anomaly detection method described above may be performed. Alternatively, in other embodiments, processor 11 may be configured to perform the geometry center pose determination method by any other suitable means (e.g., by means of firmware).

[0097] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations may include: implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0098] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0099] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0100] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0101] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or computing systems that include middleware components (e.g., application servers), or computing systems that include frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0102] A computing system can include clients and servers. Clients and servers are generally geographically separated and typically interact via communication networks. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a host product within the cloud computing service system. This addresses the shortcomings of traditional physical hosts and dedicated virtual services, such as high management difficulty and weak business scalability.

[0103] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0104] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method of determining the pose of a geometric body center, characterized by, include: Obtain the visual detection results of the target geometry in the current frame, as sent by the vision system; The visual detection results are traversed to extract at least one target QR code and the position and pose information corresponding to the target QR code; the position and pose information includes position coordinates and target pose quaternions. The position and attitude information is calculated based on the rotation mapping table and the attitude offset vector list to obtain the candidate center pose information corresponding to the target QR code; Based on the standard pose information, the candidate center pose information is used to perform end-effector localization calculation to determine the target center pose information.

2. The method according to claim 1, characterized in that, The calculation of the position and pose information based on the rotation mapping table and the pose offset vector list to obtain the candidate center pose information corresponding to the target QR code includes: The target QR code is parsed to obtain the target QR code identifier; The center attitude quaternion is determined based on the target QR code identifier, the rotation mapping table, and the target attitude quaternion. Based on the target QR code identifier, determine the attitude offset vector corresponding to the target QR code identifier from the attitude offset vector list; The target offset vector is determined based on the target attitude quaternion and the attitude offset vector; The center position coordinates of the target geometry are determined based on the target offset vector and the position coordinates. Candidate center pose information is determined based on the center position coordinates and the center pose quaternion.

3. The method according to claim 2, characterized in that, The process of determining the center pose quaternion based on the target QR code identifier, the rotation mapping table, and the target pose quaternion includes: If the target QR code is a reference surface identifier, then the target attitude quaternion corresponding to the target QR code is used as the center attitude quaternion. If the target QR code identifier is not the reference face identifier, then the rotation quaternion corresponding to the target QR code identifier is determined from the rotation mapping table based on the target QR code identifier; The center attitude quaternion is determined based on the rotation quaternion and the target attitude quaternion.

4. The method according to claim 1, characterized in that, The step of performing end-effector localization calculation on candidate center pose information based on standard pose information to determine target center pose information includes: The standard pose information and the candidate center pose information are compared. If the standard pose information and the candidate center pose information are inconsistent, the pose error is determined based on the standard pose information and the candidate center pose information. The pose adjustment amount for the current frame is calculated based on the pose error. The pose adjustment amount of the current frame is superimposed with the center pose information of the previous frame to obtain the target center pose information of the current frame.

5. The method according to claim 4, characterized in that, After superimposing the pose adjustment amount of the current frame with the center pose information of the previous frame to obtain the target center pose information of the current frame, the method further includes: If there are at least two target center pose information, then the mean position coordinates of at least one center position are calculated to obtain the mean position coordinates; The position error is determined based on the mean position coordinates and the at least one center position coordinates; The mean value of at least one center attitude quaternion is calculated to obtain the average attitude quaternion. The sum of squared angle differences is calculated based on the average attitude quaternion and the at least one central attitude quaternion. If both the position error and the sum of squared angle differences are greater than the error threshold, then the rotation quaternions and attitude offset vectors in the rotation mapping table and the attitude offset vector list are optimized based on the at least one center position coordinate and the at least one center attitude quaternion, respectively.

6. The method according to claim 5, characterized in that, Based on the at least one center position coordinate and at least one center attitude quaternion, the rotation quaternions and attitude offset vectors in the rotation mapping table and the attitude offset vector list are optimized, including: The parameter vector is defined based on the rotation quaternion in the rotation mapping table and the attitude offset vector in the attitude offset vector list; Construct a least-squares optimization function based on the parameter vector and the at least one target center pose information; If there are two center position coordinates, then determine the attitude offset vector corresponding to the center position coordinates; The attitude offset vector is geometrically corrected based on the center position coordinates and the least squares optimization function, respectively. If there are at least three center position coordinates, then determine the number of position and attitude information and the initial parameters of the least squares optimization function; The residual vector is determined based on different center position coordinates; The initial parameters of the least squares optimization function are iteratively optimized based on the amount of position and attitude information and the residual vector to obtain the optimized parameters.

7. A device for determining the center pose of a geometric body, characterized in that, include: The visual detection result acquisition module is used to acquire the visual detection results of the target geometry in the current frame issued by the vision system; The pose extraction module is used to traverse the visual detection results and extract at least one target QR code and the position and pose information corresponding to the target QR code; the position and pose information includes position coordinates and target pose quaternions. The pose calculation module is used to calculate the position and pose information based on the rotation mapping table and the pose offset vector list to obtain the candidate center pose information corresponding to the target QR code. The pose determination module is used to perform end-effector localization calculation on the candidate center pose information based on standard pose information to determine the target center pose information.

8. An electronic device, characterized in that, The electronic device includes: At least one processor; and A memory communicatively connected to the at least one processor; wherein, The memory stores a computer program that can be executed by the at least one processor, the computer program being executed by the at least one processor to enable the at least one processor to perform the geometry center pose determination method according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that cause a processor to execute the method for determining the geometric center pose of any one of claims 1-6.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the method for determining the pose of the geometric center according to any one of claims 1-6.

Images