A device and method for acquiring a six-degree-of-freedom pose of a target rigid body in a workspace

Abstract

The application relates to a device and a method for acquiring the six-degree-of-freedom pose of a target rigid body in a workspace, wherein the workspace is provided with a mechanical device and a workbench, the device for acquiring the six-degree-of-freedom pose of the target rigid body comprises a target rigid body, a reference rigid body, a positioning camera and a controller, an operating tool is installed on the operating end of the mechanical device, the target rigid body is installed on the mechanical device at a position close to the operating tool, the reference rigid body is installed on the workbench, the positioning camera is installed on the mechanical device and moves along with the mechanical device, the target rigid body and the reference rigid body are located in the lens field of view of the positioning camera, the positioning camera communicates with the controller, and the controller acquires the accurate position data of the target rigid body according to the pose data of the target rigid body and the reference rigid body collected by the positioning camera. Compared with the prior art, the application has the advantages of low arrangement difficulty, good robustness, convenient installation, high positioning accuracy and high frame rate.

Description

Technical Field

[0001] This invention relates to the field of target rigid body positioning technology in workspace, and in particular to a device and method for acquiring the six-degree-of-freedom pose of a target rigid body in workspace. Background Technology

[0002] Using automated mechanical devices (such as robotic arms) to weld and assemble parts on workbenches has become an indispensable technology in industrial automation. More and more factories (including automobile manufacturing, electronics processing, textiles, etc.) are using mechanical devices to replace manual labor in production, increasing capacity and reducing costs. Therefore, positioning the operating end of the robotic arm (such as welding drill bits, robotic arms for grasping objects, etc.) has become a crucial step in mechanical manufacturing. Taking welding as an example, only by accurately and in real-time obtaining the position of the drill bit can the robotic arm be given clear instructions to perform welding. The accuracy of positioning directly affects the welding deviation and the manufacturing process.

[0003] Currently, many mechanical devices on the market have built-in positioning functions, which can accurately locate the position and rotation of the mechanical device within its own coordinate system. However, in some cases, due to factors such as slight offset caused by contact and friction between the mechanical device and the worktable, the positioning accuracy becomes unstable and decreases. Therefore, many manufacturers use machine vision methods to reposition the end effector of the mechanical device. A common practice is to fix two or more cameras (color, infrared, and depth cameras are all used) at the end effector, with the number of cameras adjusted according to the required spatial range. Binocular stereo vision or triangulation techniques combined with infrared lasers are used to locate the end effector. This approach can indeed compensate for the inherent instability of the mechanical device's positioning, but these traditional machine vision methods also have some limitations:

[0004] 1. The above method requires converting the calculated mechanical end position in the camera system to the mechanical device's coordinate system. Generally, a calibration is performed beforehand to establish the transformation between these two coordinate systems. This means that once the camera and mechanical device are installed, they cannot be moved, otherwise the transformation relationship between the two coordinate systems will be disrupted. Thus, the area illuminated by the camera is fixed and does not change with the movement of the mechanical device, resulting in a relatively limited positioning space. To increase the positioning space, it is necessary to increase the number of cameras, thereby increasing costs.

[0005] 2. Systems using stereo cameras typically employ feature points or deep learning to identify mechanical drill bits and then design complex algorithms to pinpoint their location. This approach is highly sensitive to lighting and background complexity, making it unsuitable for high-precision positioning. While camera-based triangulation using lasers or projection gratings can mitigate the effects of ambient light, the limited scanning range of a single laser scan and the inherent difficulty in balancing scanning speed and ambient light resistance remain challenges for structured light-based systems. In other words, achieving a balance between positioning accuracy and frame rate is challenging. Furthermore, lasers exhibit irregular refraction and reflection from some metals, increasing uncertainty. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of the prior art and provide a device and method for acquiring the six-degree-of-freedom pose of a rigid body in a workspace that is easy to set up, robust, convenient to install, has high positioning accuracy, and a high frame rate.

[0007] The objective of this invention can be achieved through the following technical solutions:

[0008] A target rigid body six-DOF pose acquisition device is disclosed in a workspace. The workspace includes a mechanical device and a worktable. The target rigid body six-DOF pose acquisition device includes a target rigid body, a reference rigid body, a positioning camera, and a controller. An operating tool is installed at the operating end of the mechanical device. The target rigid body is fixedly installed at the end of the mechanical device near the operating tool. The reference rigid body is fixedly installed on the worktable. The positioning camera is installed at the other end of the mechanical device and moves with the mechanical device. The target rigid body and the reference rigid body are located within the field of view of the positioning camera. The positioning camera communicates with the controller, and the controller obtains the precise position data of the target rigid body based on the pose data of the target rigid body and the reference rigid body acquired by the positioning camera.

[0009] Preferably, both the target rigid body and the reference rigid body are rigid bodies made of active marker points.

[0010] More preferably, the active marker point is specifically an active infrared LED light.

[0011] More preferably, the number of marker points in both the target rigid body and the reference rigid body is 3 to 8.

[0012] Preferably, the positioning camera is a binocular camera.

[0013] A method for acquiring the six-DOF pose of a rigid body target in a target rigid body six-DOF pose acquisition device within the aforementioned workspace, the method being embedded within a controller, the method comprising:

[0014] Step 1: Determine if a parameter optimization instruction has been received. If yes, proceed to Step 2; otherwise, proceed directly to Step 3.

[0015] Step 2: Optimize the parameters, then proceed to Step 3;

[0016] Step 3: Acquire the rotation and position data of the target rigid body using a positioning camera [R] zt ,t zt ];

[0017] Step 4: Acquire the rotation and position data of the reference rigid body using a positioning camera [R] ref ,t ref After conversion, the six-degree-of-freedom pose of the positioning camera relative to the worktable is obtained;

[0018] Step 5: Transform the pose of the operating tool from the camera coordinate system to the world coordinate system of the working table by locating the six-DOF pose of the positioning camera relative to the worktable, thereby obtaining the precise X-axis position of the operating tool. zt .

[0019] Preferably, the triggering condition for the parameter optimization instruction in step 1 is:

[0020] The initial stage of setting up the target rigid body six-DOF pose acquisition device and after the target rigid body or reference rigid body is rearranged.

[0021] Preferably, step 2 specifically comprises:

[0022] Collect N sets of data, each set including rotation and position data of the target rigid body acquired by the dynamic positioning camera [R] zt ,t zt ], Reference rigid body rotation and position data [R ref ,t ref And the true value of the position X of the operating tool in the coordinate system of the mechanical device. zt_r ;

[0023] The variables that need to be optimized include: the rotation data of the reference rigid body in the table coordinate system. and location data And the offset Δt between the operating tool and the center of gravity of the target rigid body;

[0024] The optimized energy function is:

[0025]

[0026] Where, x zt_m The location of the operating tool; T rm The transformation matrix from the positioning camera to the coordinate system of the mechanical device;

[0027]

[0028] The energy function is then:

[0029]

[0030] The objective function is to minimize the energy function.

[0031] More preferably, the method for solving the energy function is as follows:

[0032] The optimal solution of the energy function is obtained by using nonlinear optimization LM iteration, which minimizes the energy function.

[0033] set up The Jacobian matrices corresponding to the three optimization variables Δt and Δt are J1, J2 and J3, respectively;

[0034] Then the Jacobian matrix J is:

[0035] J = [J1 J2 J3]

[0036]

[0037]

[0038]

[0039]

[0040] P = R ref -1 (R zt Δt+t zt -t ref )

[0041] in, This is the derivative of the rotation matrix with respect to Rodrigues.

[0042] Preferably, step 5 specifically comprises:

[0043]

[0044] Among them, X zt This refers to the three-dimensional position of the operating tool in the coordinate system of the worktable. and These are the rotation and position data of the reference rigid body in the worktable coordinate system, respectively; R ref The rotational data of the reference rigid body acquired by the positioning camera; t ref The position data of the reference rigid body acquired by the positioning camera; R zt For positioning camera acquisition of target rigid body rotation data; t ztThe position data of the target rigid body acquired by the positioning camera; Δt is the offset of the operating tool from the center of gravity of the target rigid body.

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] 1. Low deployment difficulty: The target rigid body six-DOF pose acquisition device in the workspace of this invention can cope with scenarios where the relative positions of the camera and the mechanical device operation section are constantly changing. The positioning camera is fixed on a certain part of the mechanical device. When the mechanical device is performing automated processing, the positioning camera can move with the mechanical device. This flexible design allows the camera to cover more areas, making the workspace of the positioning device larger. Only a small number of positioning cameras are needed to obtain a larger positioning area, reducing the positioning cost of the system and reducing the deployment difficulty of the positioning device.

[0047] II. Good robustness: The target rigid body six-DOF pose acquisition device in the workspace of the present invention uses a binocular camera to capture the rotation and position data of the target rigid body and the reference rigid body. The movement is not subject to camera tracking of the rigid body composed of active infrared light. It is resistant to ambient light interference, has strong anti-occlusion ability, and the positioning device has strong robustness.

[0048] 3. Easy installation: The target rigid body six-degree-of-freedom pose acquisition device in the workspace of the present invention does not need to be recalibrated after the positioning camera moves. Recalibration is only required when the target rigid body or reference rigid body is rearranged.

[0049] IV. High positioning accuracy: The target rigid body six-degree-of-freedom pose acquisition device and method in the workspace of the present invention has high positioning accuracy for the operating tool. Experiments have shown that the positioning accuracy is between 0.1 and 0.3 mm.

[0050] V. High frame rate: The target rigid body six-DOF pose acquisition method in the workspace of this invention has low computational complexity, and the positioning frame rate mainly depends on the frame rate of the binocular camera, which can generally reach 120-210 frames per second or higher. Attached Figure Description

[0051] Figure 1 This is a schematic diagram of the target rigid body six-degree-of-freedom pose acquisition device in this invention;

[0052] Figure 2 This is a flowchart illustrating the method for obtaining the pose of a target rigid body with six degrees of freedom in this invention.

[0053] The numbers in the diagram are as follows:

[0054] 1. Mechanical device, 2. Target rigid body, 3. Reference rigid body, 4. Worktable, 5. Positioning camera, 6. Operating tools. Detailed Implementation

[0055] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. 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.

[0056] A target rigid body six-DOF pose acquisition device in a workspace, the structure of which is as follows: Figure 1 As shown, the workspace includes a mechanical device 1 and a worktable 4. The six-degree-of-freedom pose acquisition device includes a target rigid body 2, a reference rigid body 3, a positioning camera 5, and a controller. An operating tool 6 is installed at the operating end of the mechanical device 1. The target rigid body 2 is installed on the mechanical device 1 near the operating tool 6. The reference rigid body 3 is installed on the worktable 4. The positioning camera 5 is installed on the mechanical device 1 and moves with the mechanical device 1. The target rigid body 2 and the reference rigid body 3 are located within the field of view of the lens of the positioning camera 5. The positioning camera 5 communicates with the controller. The controller obtains the precise position data of the target rigid body 2 based on the pose data of the target rigid body 2 and the reference rigid body 3 collected by the positioning camera 5.

[0057] In this embodiment, both the target rigid body 2 and the reference rigid body 3 are rigid bodies made of active markers. The active markers are active infrared LEDs, and the number of markers in both the target rigid body 2 and the reference rigid body 3 is 3 to 8. At the same time, the positioning camera 5 is a binocular camera, which is a professional camera that can accurately acquire the position of markers made of special materials in space, and can accurately acquire the position of the target rigid body 2 and the reference rigid body 3.

[0058] This embodiment also relates to a method for acquiring the six-DOF pose of a target rigid body in the aforementioned workspace using a target rigid body six-DOF pose acquisition device. This method is embedded within the controller, and its process is as follows: Figure 2 As shown, it includes:

[0059] Step 1: Determine if a parameter optimization instruction has been received. If yes, proceed to Step 2; otherwise, proceed directly to Step 3.

[0060] The parameter optimization command is triggered when the target rigid body six-DOF pose acquisition device is set up in the initial stage and after the target rigid body or reference rigid body is rearranged. That is, parameter optimization is performed in the initial stage of device setup. If the target rigid body or reference rigid body is not rearranged and installed subsequently, no optimization is required.

[0061] Step 2: Optimize the parameters, then proceed to Step 3;

[0062] Collect N sets of data, each set including rotation and position data of the target rigid body acquired by the dynamic positioning camera [R] zt ,t zt ], Reference rigid body rotation and position data [R ref ,t ref And the true value of the position X of the operating tool in the coordinate system of the mechanical device. zt_r ;

[0063] The variables that need to be optimized include: the rotation data of the reference rigid body in the table coordinate system. and location data And the offset Δt between the operating tool and the center of gravity of the target rigid body;

[0064] The optimized energy function is:

[0065]

[0066] Where, x zt_m The location of the operating tool; T rm The transformation matrix from the positioning camera to the coordinate system of the mechanical device;

[0067]

[0068] The energy function is then:

[0069]

[0070] The objective function is to minimize the energy function;

[0071] The optimal solution of the energy function is obtained by using nonlinear optimization LM iteration, which minimizes the energy function.

[0072] set up The Jacobian matrices corresponding to the three optimization variables Δt and Δt are J1, J2 and J3, respectively;

[0073] Then the Jacobian matrix J is:

[0074] J = [J1 J2 J3]

[0075]

[0076]

[0077]

[0078]

[0079] P = R ref -1 (R zt Δt+tzt -t ref )

[0080] in, For the derivative of the rotation matrix with respect to Rodrigues;

[0081] Step 3: Acquire the rotation and position data of the target rigid body using a positioning camera [R] zt ,t zt ];

[0082] Step 4: Acquire the rotation and position data of the reference rigid body using a positioning camera [R] ref ,t ref After conversion, the six-degree-of-freedom pose of the positioning camera relative to the worktable is obtained;

[0083] Step 5: Transform the pose of the operating tool from the camera coordinate system to the world coordinate system of the working table by locating the six-DOF pose of the positioning camera relative to the worktable, thereby obtaining the precise X-axis position of the operating tool. zt Specifically:

[0084]

[0085] Among them, X zt This refers to the three-dimensional position of the operating tool in the coordinate system of the worktable. and These are the rotation and position data of the reference rigid body in the worktable coordinate system, respectively; R ref The rotational data of the reference rigid body acquired by the positioning camera; t ref The position data of the reference rigid body acquired by the positioning camera; R zt For positioning camera acquisition of target rigid body rotation data; t zt The position data of the target rigid body acquired by the positioning camera; Δt is the offset of the operating tool from the center of gravity of the target rigid body.

[0086] Here is a specific application example:

[0087] The mechanical device is a robotic arm, and the operating tool is a drill bit. Precise positioning of the drill bit, mounted at the end of the robotic arm, is required. A binocular camera is mounted on the skeleton of the robotic arm. The target rigid body is installed near the drill bit, and the reference rigid body is welded to the worktable. Alternatively, other methods of fixing and connecting, such as bonding, can also be used. Both the target and reference rigid bodies use actively emitting infrared LEDs, with each rigid body consisting of 3 to 8 infrared LEDs. Two binocular cameras are used, placed parallel or nearly parallel at a suitable distance, with a baseline distance generally between 15 and 100 cm. The relative pose between the two cameras remains constant. The cameras should be positioned to capture the two rigid bodies in the system. The two binocular cameras are model MC1300.

[0088] First, the binocular camera itself is calibrated in the motion capture software. In this embodiment, the T-bar calibration method is used. Then, two rigid bodies are manually created in the motion capture software. Subsequently, approximately 10-20 sets of data are collected for parameter optimization. Each set of data includes the rotation and position data of the two rigid bodies obtained by the binocular camera tracking, as well as the true value of the drill bit position in the worktable coordinate system. After parameter optimization is completed, it can be put into use to obtain accurate drill bit positioning data in real time.

[0089] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for acquiring the six-DOF pose of a target rigid body in a workspace, the method being embedded within a controller, characterized in that, The workspace is equipped with a mechanical device (1) and a workbench (4); the target rigid body six-degree-of-freedom pose acquisition device includes a target rigid body (2), a reference rigid body (3), a positioning camera (5) and a controller; the operating end of the mechanical device (1) is equipped with an operating tool (6); the target rigid body (2) is installed on the mechanical device (1) near the operating tool (6); the reference rigid body (3) is installed on the workbench (4); the positioning camera (5) is installed on the mechanical device (1) and moves with the mechanical device (1); the target rigid body (2) and the reference rigid body (3) are located within the field of view of the lens of the positioning camera (5); the positioning camera (5) communicates with the controller, and the controller obtains the precise position data of the target rigid body (2) based on the pose data of the target rigid body (2) and the reference rigid body (3) collected by the positioning camera (5); The method for obtaining the six-DOF pose of the target rigid body includes: Step 1: Determine if a parameter optimization instruction has been received. If yes, proceed to Step 2; otherwise, proceed directly to Step 3. Step 2: Optimize the parameters, then proceed to Step 3; Step 3: Acquire rotation and position data of the target rigid body using a positioning camera. ; Step 4: Acquire the rotation and position data of the reference rigid body using a positioning camera. After conversion, the six-degree-of-freedom pose of the positioning camera relative to the worktable is obtained; Step 5: Transform the pose of the operating tool from the camera coordinate system to the world coordinate system of the working table by locating the six-DOF pose of the camera relative to the worktable, thereby obtaining the precise positioning of the operating tool. ; Step 2 specifically refers to: collection Each set of data includes rotation and position data of the target rigid body acquired by the dynamic positioning camera. Reference rigid body rotation and position data and the true position of the operating tool in the coordinate system of the mechanical device ; The variables that need to be optimized include: the rotation data of the reference rigid body in the table coordinate system. and location data And the offset of the tool from the center of gravity of the target rigid body ; The optimized energy function is: in, The location of the operating tool; The transformation matrix from the positioning camera to the coordinate system of the mechanical device; The energy function is then: The objective function is to minimize the energy function.

2. The method for acquiring the six-DOF pose of a rigid body target in a workspace, as described in claim 1, is characterized in that... The triggering condition for the parameter optimization instruction in step 1 is: The initial stage of setting up the target rigid body six-DOF pose acquisition device and after the target rigid body or reference rigid body is rearranged.

3. The method for acquiring the six-DOF pose of a rigid body target in a workspace, as described in claim 1, is characterized in that... The method for solving the energy function is as follows: The optimal solution of the energy function is obtained by using nonlinear optimization LM iteration, which minimizes the energy function. set up , and The Jacobian matrices corresponding to these three optimization variables are as follows: , and ; Then the Jacobian matrix for: in, This is the derivative of the rotation matrix with respect to Rodrigues.

4. The method for acquiring the six-DOF pose of a rigid body target in a workspace according to claim 1, characterized in that, Step 5 specifically includes: in, This refers to the three-dimensional position of the operating tool in the coordinate system of the worktable. and These are the rotation and position data of the reference rigid body in the worktable coordinate system, respectively. The rotational data of the reference rigid body acquired by the positioning camera; Position data of the reference rigid body acquired by the positioning camera; For positioning camera acquisition of target rigid body rotation data; Position data of the target rigid body acquired by the positioning camera; This is the offset of the operating tool from the center of gravity of the target rigid body.

5. The method for acquiring the six-DOF pose of a rigid body target in a workspace according to claim 1, characterized in that, The target rigid body (2) and the reference rigid body (3) are both rigid bodies made of active marker points.

6. The method for acquiring the six-DOF pose of a rigid body target in a workspace according to claim 5, characterized in that, The active marker point is specifically an active infrared LED light.

7. The method for acquiring the six-DOF pose of a rigid body target in a workspace, as described in claim 5, is characterized in that... The number of marker points in both the target rigid body (2) and the reference rigid body (3) is 3 to 8.

8. The method for acquiring the six-DOF pose of a rigid body target in a workspace according to claim 1, characterized in that, The positioning camera (5) is specifically a binocular camera.

Images