Robot space attitude resolving method and system based on multi-light spot image

Abstract

The invention discloses a robot space attitude resolving method and system based on a multi-light-spot image. The resolving method comprises the following steps: collecting laser spot image information emitted by a plurality of laser displacement distance measuring sensors through a depth camera; an internal reference matrix K from an image coordinate system to a camera coordinate system and a homogeneous transformation matrix from the camera coordinate system to a mechanical arm tail end tool coordinate system are obtained; solving to obtain extrinsic parameters of multiple groups of laser displacement distance measuring sensors relative to a camera coordinate system; and constructing a rotation matrix from a mechanical arm tail end tool coordinate system to a plane coordinate system by combining the homogeneous transformation matrix, extrinsic parameters of a plurality of groups of laser displacement distance measuring sensors relative to a camera coordinate system and laser distance measuring distance values. The space coordinates of the plane where the multiple laser spots are located can be solved through the laser ranging distance values and the external parameters, and the problems that the attitude calculation error is large, the attitude adjustment effect is poor and the like due to the fact that the image recognition quality is poor due to the fact that a traditional pure vision scheme is prone to being affected by light rays, shooting angles and other factors can be solved.

Description

Technical Field

[0001] The present invention relates to the technical field of computer graphics data processing, and in particular to a robot space posture solving method and system based on multi-light spot images. Background Art

[0002] At present, in order to ensure that the robot arm can accurately and autonomously locate and operate the target, it is necessary to solve the problem of how to accurately perceive the position and posture of the target. Traditional posture recognition solution methods usually rely on pre-programmed paths or simple recognition based on visual systems, but these methods have certain limitations, such as being sensitive to ambient lighting conditions and being easily affected by shooting angles, resulting in poor image recognition quality, which in turn causes large posture solution errors and unsatisfactory posture adjustment effects. For example, the auxiliary posture recognition method based on Aruco code can better obtain the posture of the plane where Aruco is located, but there are problems such as being easily affected by light interference and viewing angle limitations, resulting in poor posture recognition effects. At the same time, there are also scenes where Aruco codes cannot be affixed, which makes this method invalid. In addition, there is a method of solving the plane posture by fusing visual and depth camera point cloud data, but this method has the problem that the acquisition of point cloud data is affected by the distance blind spot, resulting in the failure of the posture solution of the robot arm within a certain range from the plane. It is also easily affected by light interference, resulting in large posture solution errors.

[0003] In summary, in order to enable the robotic arm to efficiently and accurately calculate the posture of the target plane, it is necessary to improve the traditional posture calculation method. Summary of the invention

[0004] The purpose of the present invention is to provide a robot spatial posture solution method and system based on multi-light spot images, so that the posture of the plane where the target is located can be solved efficiently and with high precision.

[0005] To achieve the purpose of the present invention, the technical solution provided by the present invention is as follows: First aspect The present invention provides a robot spatial posture solution method based on multi-light spot images, comprising the following steps: Step 1: Collect laser spot image information emitted by multiple laser displacement ranging sensors through a depth camera, wherein the multiple laser displacement ranging sensors and the depth camera are installed at the end of the robotic arm and on the same plane; Step 2: Calibrate the intrinsic and extrinsic parameters of the depth camera to obtain the intrinsic matrix from the image coordinate system to the camera coordinate system K And the homogeneous transformation matrix from the camera coordinate system to the tool coordinate system at the end of the robot arm; Step 3: Identify and process the laser spot image information, obtain the three-dimensional coordinates of the laser spot in the camera coordinate system, and calibrate multiple laser displacement ranging sensors to obtain multiple sets of laser displacement ranging sensors relative to the camera coordinate system. Step 4: Combine the homogeneous transformation matrix obtained in step 2 and the multiple sets of external parameters of the laser displacement ranging sensor relative to the camera coordinate system and the laser ranging distance value obtained in step 3 to construct a rotation matrix from the tool coordinate system at the end of the robot arm to the plane coordinate system, so that the posture of the tool coordinate system at the end of the robot arm is aligned with the posture of the plane coordinate system, and complete the robot's spatial posture solution.

[0006] Second aspect The present invention provides a robot space posture solution system based on multi-light spot images, comprising a plurality of laser displacement ranging sensors and a depth camera and a data processing unit connected to the plurality of laser displacement ranging sensors and the depth camera; the system executes the above method.

[0007] Compared with the prior art, the beneficial effects of the present invention are as follows: the spatial coordinates of the plane where multiple laser spots are located can be solved by the laser ranging distance value and the external parameter parameters, which can overcome the problems of poor image recognition quality caused by factors such as light and shooting angle when using traditional pure vision solutions, resulting in large posture solution errors and poor posture adjustment effects, so that the robotic arm can efficiently and accurately solve the posture of the plane where the target is located. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] Figure 1 A schematic flow chart of a method for calculating a robot's spatial posture based on multi-light spot images provided by an embodiment of the present invention; Figure 2 A schematic diagram of the structure of a robot space posture solving system based on multi-light spot images provided by an embodiment of the present invention; In the figure, 1-laser displacement ranging sensor, 2-depth camera. DETAILED DESCRIPTION

[0009] The following is a clear and complete description of the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0010] like Figure 1 As shown, this embodiment provides a robot spatial posture solution method based on multi-light spot images, comprising the following steps: Step 1: Collect laser spot image information emitted by multiple laser displacement ranging sensors through a depth camera, wherein the multiple laser displacement ranging sensors and the depth camera are installed at the end of the robotic arm and on the same plane; It should be noted that the laser light spot refers to a light spot formed when a laser beam emitted by a single laser displacement ranging sensor irradiates a target plane.

[0011] It should be noted that the number of laser displacement ranging sensors is preferably 3, but may also be 4, 5 or more.

[0012] Step 2: Calibrate the intrinsic and extrinsic parameters of the depth camera to obtain the intrinsic matrix from the image coordinate system to the camera coordinate system K And the homogeneous transformation matrix from the camera coordinate system to the tool coordinate system at the end of the robot arm; The details include: Step 2.1: Use the depth camera to collect chessboard image information of the robot arm in different postures, and use the depth camera internal parameter parameters to obtain the internal parameter matrix from the image coordinate system to the camera coordinate system K ; It should be noted that the depth camera installed at the end of the mobile robot collects chessboard image information at different viewing angles. In order to reduce the calibration error, as many groups of chessboard images at different viewing angles as possible are collected.

[0013] In this embodiment, 20 groups of chessboard images under random viewing angles can be collected, chessboard corner point detection can be performed using OpenCV, and the camera intrinsic parameter matrix can be calculated.

[0014] Step 2.2: Use the depth camera to collect and record the Aruco code image recognition information in multiple postures of the robot, including the spatial coordinates and Euler angles of the tool center point at the end of the robot in the robot base coordinate system, and the spatial coordinates and Euler angles of the Aruco code center point in the camera coordinate system; It should be noted that, preferably, in this embodiment, the robot arm is moved 20 times to collect 20 ArUCo two-dimensional code image information in different postures for calculation.

[0015] Step 2.3: According to the internal parameter matrix K And the Aruco code image recognition information under multiple postures of the robotic arm is used to obtain the homogeneous transformation matrix from the camera coordinate system to the tool coordinate system at the end of the robotic arm.

[0016] Step 3: Identify and process the laser spot image information, obtain the three-dimensional coordinates of the laser spot in the camera coordinate system, and calibrate multiple laser displacement ranging sensors to obtain multiple sets of laser displacement ranging sensors relative to the camera coordinate system. It should be noted that, in step 3, the OpenCV computer vision library is used to process the laser spot image information.

[0017] Specifically, the step 3 includes the following: Step 3.1: Use the grayscale centroid method module in the OpenCV computer vision library to process the laser spot image information and extract the centroid coordinates of the laser spot. The formula is as follows: (1) (2) In the formula, is the horizontal coordinate of the center of mass of the laser spot, is the ordinate of the laser spot centroid, and represents the row and column coordinates of the pixel, and Represents the total number of rows and columns of the image, is the pixel coordinate The corresponding pixel gray value; Step 3.2: Combine the internal parameter matrix K and the centroid coordinates of the laser spot in the image coordinate system and the depth information obtained from the depth camera , calculate the spatial coordinates of the laser spot in the camera coordinate system , the formula is as follows: (3) Step 3.3: Move the robotic arm multiple times to collect multiple sets of three-dimensional coordinates of the laser spots at different distances from multiple laser displacement ranging sensors in the camera coordinate system, and use the LM optimization algorithm to solve the external parameter parameters of the laser displacement ranging sensor relative to the camera coordinate system; Step 3.4: Repeat the above steps to complete the calibration of multiple sets of laser displacement ranging sensors’ external parameters.

[0018] Step 4: Combine the homogeneous transformation matrix obtained in step 2 and the multiple sets of external parameters of the laser displacement ranging sensor relative to the camera coordinate system and the laser ranging distance value obtained in step 3 to construct a rotation matrix from the tool coordinate system at the end of the robot arm to the plane coordinate system, so that the posture of the tool coordinate system at the end of the robot arm is aligned with the posture of the plane coordinate system, and complete the robot's spatial posture solution.

[0019] It should be noted that, in step 4, the construction of the rotation matrix from the tool coordinate system at the end of the robot arm to the plane coordinate system specifically includes the following: Step 4.1: Calculate the normal vectors of the planes where multiple laser displacement ranging sensors and depth cameras are located , transform the normal vector to the robot base coordinate system and normalize it to get , the expression is as follows: (4) In the formula, is the coordinate of the normal vector of the plane where multiple laser displacement ranging sensors and depth cameras are located in the robot base coordinate system, is the rotation matrix from the tool coordinate system at the end of the robot arm to the robot base coordinate system, Rotation matrix from the camera coordinate system to the tool coordinate system at the end of the robot arm; Step 4.2: Select the projection component of the x-axis of the tool coordinate system at the end of the robot arm on the x-axis of the robot arm base coordinate system as the projection component of the x-axis of the plane coordinate system on the x-axis of the robot arm base coordinate system , using orthogonal constraints and unit vector constraints to construct constraint equations, namely and The inner product is 0. equal and The outer product of is a unit vector and therefore satisfies The sum of the squares is 1. Solve and construct the rotation matrix from the robot base coordinate system to the plane coordinate system. , the expression is as follows: (5) (6) (7) (8) In the formula, is the projection component obtained by projecting the x-axis of the plane coordinate system on the three axes of the robot base coordinate system. , , represents the column vector obtained by projecting the x, y, and z axes of the plane coordinate system onto the x, y, and z axes of the robot base coordinate system, and , , , is the projection component obtained by projecting the y-axis of the plane coordinate system onto the three axes of the robot base coordinate system. are the projection components obtained by projecting the z-axis of the plane coordinate system onto the three axes of the robot base coordinate system, where , according to the solved rotation matrix , so that the end of the robotic arm is consistent with the plane where the target is located.

[0020] Corresponding to the above method, Figure 2As shown, an embodiment of the present invention also provides a robot spatial posture solution system based on multi-light spot images, including multiple laser displacement ranging sensors 1 and a depth camera 2 and a data processing unit (not shown in the figure) connected to the multiple laser displacement ranging sensors 1 and the depth camera 2; the system executes the method as described above.

[0021] It should be noted that the number of laser displacement ranging sensors is preferably 3, but may also be 4, 5 or more.

[0022] Finally, it should be noted that the above embodiments are only used to illustrate and explain the present invention, and are not intended to limit the present invention to the scope of the described embodiments. In addition, those skilled in the art can understand that the present invention is not limited to the above embodiments, and more variations and modifications can be made according to the teachings of the present invention, and these variations and modifications all fall within the scope of the protection claimed by the present invention.

Claims

1. A robot spatial posture solution method based on multi-light spot images, characterized in that: The steps include: Step 1: Collect laser spot image information emitted by multiple laser displacement ranging sensors through a depth camera, wherein the multiple laser displacement ranging sensors and the depth camera are installed at the end of the robotic arm and on the same plane; Step 2: Calibrate the intrinsic and extrinsic parameters of the depth camera to obtain the intrinsic matrix from the image coordinate system to the camera coordinate system K And the homogeneous transformation matrix from the camera coordinate system to the tool coordinate system at the end of the robot arm; Step 3: Identify and process the laser spot image information, obtain the three-dimensional coordinates of the laser spot in the camera coordinate system, and calibrate multiple laser displacement ranging sensors to obtain multiple sets of laser displacement ranging sensors relative to the camera coordinate system. Step 4: Combine the homogeneous transformation matrix obtained in step 2 and the multiple sets of external parameters of the laser displacement ranging sensor relative to the camera coordinate system and the laser ranging distance value obtained in step 3 to construct a rotation matrix from the tool coordinate system at the end of the robot arm to the plane coordinate system, so that the posture of the tool coordinate system at the end of the robot arm is aligned with the posture of the plane coordinate system, and complete the robot's spatial posture solution.

2. The robot spatial posture solution method based on multi-light spot images according to claim 1 is characterized in that: The step 2 specifically includes the following: Step 2.1: Use the depth camera to collect chessboard image information of the robot arm in different postures, and use the depth camera internal parameter parameters to obtain the internal parameter matrix from the image coordinate system to the camera coordinate system K ; Step 2.2: Use the depth camera to collect and record the Aruco code image recognition information in multiple postures of the robot, including the spatial coordinates and Euler angles of the tool center point at the end of the robot in the robot base coordinate system, and the spatial coordinates and Euler angles of the Aruco code center point in the camera coordinate system; Step 2.3: According to the internal parameter matrix K And the Aruco code image recognition information under multiple postures of the robotic arm is used to obtain the homogeneous transformation matrix from the camera coordinate system to the tool coordinate system at the end of the robotic arm.

3. The robot spatial posture solution method based on multi-light spot images according to claim 2 is characterized in that: In step 3, the laser spot image information is processed using the OpenCV computer vision library.

4. The robot spatial posture solution method based on multi-light spot images according to claim 3 is characterized in that: The step 3 specifically includes the following: Step 3.1: Use the grayscale centroid method module in the OpenCV computer vision library to process the laser spot image information and extract the centroid coordinates of the laser spot. The formula is as follows: （1） （2） In the formula, is the horizontal coordinate of the center of mass of the laser spot, is the ordinate of the laser spot centroid, and represents the row and column coordinates of the pixel, and Represents the total number of rows and columns of the image, is the pixel coordinate The corresponding pixel gray value; Step 3.2: Combine the internal parameter matrix K and the centroid coordinates of the laser spot in the image coordinate system and the depth information obtained from the depth camera , calculate the spatial coordinates of the laser spot in the camera coordinate system , the formula is as follows: （3） Step 3.3: Move the robotic arm multiple times to collect multiple sets of three-dimensional coordinates of the laser spots at different distances from multiple laser displacement ranging sensors in the camera coordinate system, and use the LM optimization algorithm to solve the external parameter parameters of the laser displacement ranging sensor relative to the camera coordinate system; Step 3.4: Repeat the above steps to complete the calibration of multiple sets of laser displacement ranging sensors’ external parameters.

5. The robot spatial posture calculation method based on multi-light spot images according to claim 4 is characterized in that: In step 4, the construction of the rotation matrix from the tool coordinate system at the end of the robot arm to the plane coordinate system specifically includes the following: Step 4.1: Calculate the normal vectors of the planes where multiple laser displacement ranging sensors and depth cameras are located , transform the normal vector to the robot base coordinate system and normalize it to get , the expression is as follows: （4） In the formula, is the coordinate of the normal vector of the plane where multiple laser displacement ranging sensors and depth cameras are located in the robot base coordinate system, is the rotation matrix from the tool coordinate system at the end of the robot arm to the robot base coordinate system, Rotation matrix from the camera coordinate system to the tool coordinate system at the end of the robot arm; Step 4.2: Select the projection component of the x-axis of the tool coordinate system at the end of the robot arm on the x-axis of the robot arm base coordinate system as the projection component of the x-axis of the plane coordinate system on the x-axis of the robot arm base coordinate system , use orthogonal constraints and unit vector constraints to construct constraint equations, solve and construct the rotation matrix from the robot base coordinate system to the plane coordinate system , the expression is as follows: （5） （6） （7） （8） In the formula is the projection component obtained by projecting the x-axis of the plane coordinate system on the three axes of the robot base coordinate system. , , represents the column vector obtained by projecting the x, y, and z axes of the plane coordinate system onto the x, y, and z axes of the robot base coordinate system, and , , , is the projection component obtained by projecting the y-axis of the plane coordinate system onto the three axes of the robot base coordinate system. are the projection components obtained by projecting the z-axis of the plane coordinate system onto the three axes of the robot base coordinate system, where , according to the solved rotation matrix , so that the end of the robotic arm is consistent with the plane where the target is located.

6. A robot spatial posture solving system based on multi-light spot images, characterized in that: The system comprises a plurality of laser displacement ranging sensors and a depth camera and a data processing unit connected to the plurality of laser displacement ranging sensors and the depth camera; the system executes the method as described in any one of claims 1-5.

7. The robot spatial posture solving system based on multi-light spot images according to claim 6, characterized in that: The number of the laser displacement ranging sensors is 3.

Images