Infrared-based quadruped robot positioning method

Abstract

An infrared-based quadruped robot positioning method comprises the following steps: S100, calibrating internal parameters and external parameters of an infrared camera, and resolving the spatial pose of the camera; s200, infrared light on the quadruped robot is captured, and coordinates of the center of mass of an aperture are calculated through edge detection; s300, obtaining the space coordinates of the three light beads by using the matching condition of the same light spot under different camera images and combining triangulation; and S400, calculating the position information of the quadruped robot in the space by using the geometrical relationship among the three light beads. According to the infrared-based quadruped robot positioning method, infrared light emitted by three infrared lamp beads on a quadruped robot body is tracked and captured through a plurality of infrared cameras, and real-time position information of the quadruped robot is calculated through algorithms such as polar line matching and triangulation; therefore, real position information with enough precision is obtained, and the manufacturing cost of the whole system is relatively low.

Description

TECHNICAL FIELD

[0001] The application relates to a robot positioning technology, in particular to an infrared-based quadruped robot positioning method. BACKGROUND

[0002] With the continuous development of industrial automation, quadruped robots gradually play a role in the industry. For example, they are responsible for industrial inspection, emergency rescue and other tasks. In order to accurately complete various tasks, the quadruped robot needs to have efficient and accurate environmental perception and positioning capabilities. At present, SLAM algorithm is usually used to realize the perception and positioning function.

[0003] When the quadruped robot is positioned by using the SLAM algorithm and other perception and positioning algorithms in an actual scene, the accuracy of the related positioning algorithm in the scene needs to be evaluated. At this time, the accurate position information of part of the running track of the quadruped robot in the scene, that is, the Ground Truth, is compared with the positioning result of the perception and positioning algorithm (such as the SLAM algorithm), and the accuracy of the perception and positioning algorithm used by the quadruped robot is evaluated.

[0004] At present, the commonly used method for obtaining the Ground Truth is the motion capture system and GPS. The motion capture system collects the reflective marker points on the quadruped robot through a high-precision infrared camera, calculates the position data, and the accuracy is usually sub-millimeter level, but the price of each hardware device and special software is relatively high, and the cost is too high for individuals or small teams. The GPS positioning system relies on satellite signals to obtain the position information of the quadruped robot, and although the price is appropriate, the accuracy is low, usually in the meter level, and the performance is worse in the shielding environment, which is not suitable for indoor environment. Therefore, a positioning scheme with appropriate price and sufficient accuracy is needed to obtain the Ground Truth, that is, the real position information of the quadruped robot in the actual environment. SUMMARY

[0005] In view of the above defects, the purpose of the present application is to provide an infrared-based quadruped robot positioning method. The real-time position information of the quadruped robot is calculated by using the epipolar matching and triangulation algorithms to track and capture the infrared light emitted by the three infrared lamp beads on the quadruped robot through multiple infrared cameras, so as to obtain the real position information with sufficient accuracy, and the cost of the whole system is also relatively low.

[0006] In order to achieve this purpose, the application adopts the following technical scheme:

[0007] An infrared-based quadruped robot positioning method comprises the following steps:

[0008] S100, calibrate the internal parameters and external parameters of the infrared camera, and solve the spatial pose of the camera;

[0009] S200 captures the infrared light on the quadruped robot and calculates the centroid coordinates of the aperture through edge detection;

[0010] S300: By utilizing the matching situation between the same light point in images from different cameras, combined with triangulation, the spatial coordinates of the three light beads are obtained;

[0011] S400: The position information of the quadruped robot in space is calculated using the geometric relationship between the three light beads.

[0012] Preferably, in step S100:

[0013] The intrinsic parameters of an infrared camera include focal length, principal point coordinates, and distortion coefficients;

[0014] The extrinsic parameters of an infrared camera represent the camera's pose in the world coordinate system;

[0015] The internal parameters of an infrared camera are usually calibrated at the factory, while the external parameters of an infrared camera are calibrated using a checkerboard calibration plate.

[0016] The PnP method in 2D-3D pose estimation is used to solve the PnP problem by minimizing the reprojection error, thereby solving the spatial pose of the camera.

[0017] Preferably, step S100 includes the following steps:

[0018] Two parallel infrared cameras are used to obtain the pixel coordinates u of each corner point of the chessboard grid by photographing the chessboard calibration board. i And establish a chessboard coordinate system based on the vertices of the chessboard grid;

[0019] Since the size of each grid cell is known, the three-dimensional coordinates P of each corner point in the chessboard coordinate system can be determined. i And the scale factor s, thus obtaining the 2D-3D corresponding points;

[0020] The extrinsic parameter of the infrared camera is the transformation matrix T from the chessboard coordinate system to the camera coordinate system, and the intrinsic parameter of the infrared camera is K; the transformation matrix T is optimized by minimizing the reprojection error e through equation (1);

[0021]

[0022] This method can obtain the transformation matrix T of the two infrared cameras relative to the chessboard coordinate system. 1c T 2c ;

[0023] To define camera one as the world coordinate system, the extrinsic parameters of camera one can be... Let the identity matrix be denoted as , then the transformation matrix of camera 2 relative to camera 1 is .

[0024] Further, in step S200, infrared light emitted by the three infrared lamp beads is captured using an infrared camera;

[0025] S210, convert the RGB image captured by the camera into a grayscale image, and set a grayscale threshold for binary processing;

[0026] S220, in the binary image, scan from the top left corner to find the first foreground pixel, and the foreground pixel has a pixel value of 255, which is a white pixel; take the foreground pixel as the starting point, and track and record the boundary points along the foreground pixel boundary;

[0027] S230, after obtaining all the infrared light contours, calculate the pixel centroid point in the contour area as the pixel coordinates of the infrared lamp bead, and the calculation formula is as follows:

[0028]

[0029] Where R is the contour radius, x and y are the horizontal and vertical coordinates of the pixel coordinates, I (x,y) is the pixel value of the specified point, c x , c y , that is, the pixel centroid coordinate value.

[0030] Further, in step S300, including:

[0031] Step S310, after extracting the infrared light centroid coordinates from the frame images of the two cameras, it is necessary to match the three points of the two infrared cameras respectively for subsequent calculation of the actual spatial coordinates of the three lamp beads using the triangulation method. The point matching method based on epipolar line is adopted, that is, by calculating the epipolar line of the point in camera one corresponding to the point in camera two, more accurate point matching is realized.

[0032] Further, in step S310, the point matching method based on epipolar line is as follows:

[0033] In the epipolar geometry between the two cameras, p1 and p2 points on the imaging planes of the two cameras are the projections of the space point p, that is, p1 and p2 are a pair of matching points;

[0034] The epipolar lines l1 and l2 are the intersection lines between the epipolar plane O1O2P and the two image planes I1 and I2, and they are in a corresponding relationship, so that the epipolar line l2 can be solved by determining the p1 point and the fundamental matrix F between the two cameras;

[0035] The fundamental matrix F describes the relationship between the two cameras, which is composed of the camera intrinsic parameter K and the camera extrinsic parameter t ^ R, which has been obtained in camera pose calibration;

[0036] F=K-T t^RK -1 Equation (3)

[0037] l2=Fp1

[0038] Using the above formula, we can derive the equation for point p1 in camera one corresponding to the epipolar line l2 in camera two. If the camera's intrinsic and extrinsic parameters and infrared point detection accuracy are high enough, then point p2 in camera two will fall on the epipolar line l2, thus p1 and p2 can be regarded as matching points.

[0039] Furthermore, step S300 also includes the following steps:

[0040] According to the definition of epipolar geometry, let x1 and x2 be the normalized coordinates of two feature points, then we have:

[0041] s1x1=s2Rx2+t Equation (4)

[0042] The rotation matrix R and translation vector t between the two cameras have been obtained through camera pose calibration, so the two depth values ​​s1 and s2 can be solved.

[0043]

[0044] After obtaining the actual depth information of the three infrared LEDs using triangulation, the spatial coordinates of these three infrared LEDs in the camera's coordinate system can be determined; and the centroid position of these three infrared LEDs can be used as the spatial position of the quadruped robot itself.

[0045] Further, step S400 includes the following steps:

[0046] Define three LED beads as ABC, with centroid D. The spatial coordinates of points A, B, and C are given as (A...). x A y A z (B) x B y B z ), (C x C y C z It can be achieved through spatial geometric relationships. Determine the spatial coordinates of the centroid D (D x D y D z )

[0047] One of the technical solutions in the above technical scheme includes the following beneficial effects: the four-legged robot is positioned by using the infrared camera that captures infrared light of a specific wavelength, which can effectively filter out the interference of ambient light, effectively overcoming the limitations of traditional visual positioning under complex lighting conditions, the specific capture of infrared waves enables the system to work stably under various lighting conditions and improves positioning accuracy, obtaining millimeter-level Ground Truth, meeting the demand for evaluating the self-environment perception positioning algorithm of the four-legged robot, and through the capture of specific wavelength infrared light and the calculation of the center of mass, the complex feature extraction and matching process in traditional computer vision is simplified, and the computational complexity is significantly reduced. At the same time, the low-cost infrared camera replaces the expensive equipment in the traditional high-precision positioning system (such as Vicon, OptiTrack), reducing hardware investment by more than 90%, and facilitating the use of individuals and small teams. BRIEF DESCRIPTION OF DRAWINGS

[0048] Figure 1 is a method step schematic diagram of an embodiment of the present application;

[0049] Figure 2 is a re-projection error schematic diagram of an embodiment of the present application;

[0050] Figure 3 is an epipolar geometry schematic diagram of an embodiment of the present application;

[0051] Figure 4 is an epipolar line matching diagram of a point pair between two infrared cameras of an embodiment of the present application;

[0052] Figure 5 is a triangulation to obtain spatial point depth schematic diagram of an embodiment of the present application;

[0053] Figure 6 is a lamp bead barycenter position schematic diagram of an embodiment of the present application. DETAILED DESCRIPTION

[0054] Embodiments of the present application are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference signs represent the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by reference to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be understood as a limitation of the present application.

[0055] As shown in Figure 1 , an infrared-based four-legged robot positioning method includes the following steps:

[0056] S100, calibrate the internal and external parameters of the infrared camera and solve the spatial pose of the camera;

[0057] S200, capture infrared light on the quadruped robot, and calculate the aperture centroid coordinates through edge detection;

[0058] S300, obtain the spatial coordinates of the three light beads through triangulation combined with the matching conditions between the same light points in different camera images;

[0059] S400, calculate the position information of the quadruped robot in space by using the geometric relationship between the three light beads.

[0060] The application can effectively filter out the interference of ambient light by positioning the quadruped robot by using the infrared camera that captures infrared light of a specific wavelength, effectively overcoming the limitations of traditional visual positioning in complex lighting conditions. The specific capture of infrared band enables the system to work stably under various lighting conditions and improves positioning accuracy, obtaining millimeter-level GroundTruth to meet the demand for evaluating the self-environment perception and positioning algorithm of the quadruped robot. Through the capture of specific wavelength infrared light and centroid calculation, the complex feature extraction and matching process in traditional computer vision is simplified, and the computational complexity is significantly reduced. At the same time, the low-cost infrared camera replaces the expensive equipment in the traditional high-precision positioning system (such as Vicon, OptiTrack), reducing hardware investment by more than 90%, and facilitating personal and small team use.

[0061] In step S100:

[0062] The internal parameters of the infrared camera include focal length, principal point coordinates and distortion coefficients;

[0063] The external parameters of the infrared camera represent the pose of the camera in the world coordinate system;

[0064] The internal parameters of the infrared camera are usually calibrated at the factory, and the external parameters of the infrared camera are calibrated by a checkerboard calibration plate;

[0065] The PnP method in the 2D-3D pose estimation method is used to solve the PnP problem by minimizing the re-projection error to calculate the spatial pose of the camera.

[0066] The infrared camera used in the scheme is equipped with a 940nm infrared light filter to effectively exclude environmental light interference and improve target detection accuracy. The physical characteristics of infrared spectrum and visible spectrum are separated, and the background interference is effectively suppressed through filtering technology, which is difficult to achieve by conventional RGB cameras. In order to accurately calculate the position information of the quadruped robot, the internal and external parameters of the camera need to be accurately obtained.

[0067] In step S100, the following steps are included:

[0068] Two infrared cameras are used to capture the pixel coordinates u of the corners of the checkerboard by shooting the checkerboard calibration board i , and a checkerboard coordinate system is established based on the vertices of the checkerboard grid

[0069] Since the size of each grid is known, the three-dimensional coordinates P of each corner in the checkerboard coordinate system can be determined i , and the scale factor s is obtained, thereby obtaining the 2D-3D corresponding points

[0070] The extrinsic parameters of the infrared camera are the transformation matrix T from the checkerboard coordinate system to the camera coordinate system, and the intrinsic parameters of the infrared camera are K; the transformation matrix T is optimized by calculating the minimum re-projection error e according to formula (1)

[0071]

[0072] This method can obtain the transformation matrix T 1c , T 2c , of the two infrared cameras relative to the checkerboard coordinate system

[0073] In order to define camera one as the world coordinate system, the extrinsic parameters of camera one can be set as the unit matrix, and the transformation matrix of camera two relative to camera one is

[0074] In this scheme, by using a checkerboard grid with a known physical size as a calibration reference, not only is the corresponding relationship between two-dimensional pixels and three-dimensional space realized, but also the absolute physical scale is explicitly introduced, thereby avoiding the scale uncertainty problem in traditional monocular vision systems. In the optimization process, the scale factor is solved as a parameter, thereby obtaining an accurate coordinate transformation relationship. The so-called re-projection is to convert a spatial point into a pixel coordinate through camera intrinsic and extrinsic parameters, as shown in formula (1) Figure 2 .

[0075] In step S200, the infrared light emitted by the three infrared light beads is captured using an infrared camera

[0076] In S210, the RGB image captured by the camera is converted into a grayscale image, and a grayscale threshold is set for binary processing

[0077] In S220, in the binary image, the first foreground pixel is found by scanning from the top left corner, and the pixel value of the foreground pixel is 255, which is a white pixel. The boundary points are tracked and recorded along the boundary of the foreground pixel starting from the foreground pixel

[0078] In S230, after obtaining all the infrared light contours, the pixel centroid point in the contour region is calculated as the pixel coordinates of the infrared light bead, and the calculation formula is as follows

[0079]

[0080] where R is the profile radius, x, y are the horizontal and vertical coordinates of the pixel coordinates, I (x,y) is the pixel value of the specified point, c x , c y , i.e. the pixel centroid coordinate value.

[0081] The method relies on infrared light beads for positioning, which can effectively avoid the interference of visible light sources, especially in complex lighting environments (such as backlight, strong light, low light, etc.) and still maintain stable performance. By converting the infrared RGB image into a grayscale image and setting a threshold for binarization, then scanning and tracking the boundary of the foreground pixels, the image processing process is simplified, making the algorithm run on lower computing resources and quickly identify the position of the target infrared light bead.

[0082] In addition, in step S300 includes:

[0083] Step S310, after extracting the infrared light centroid coordinates from the frame images of the two cameras, in order to calculate the actual spatial coordinates of the three light beads using the triangulation method later, it is necessary to first match the corresponding 3 points of the two infrared cameras respectively. The point matching method based on epipolar line is adopted, which realizes more accurate point matching by calculating the epipolar line of the point in camera one corresponding to the point in camera two.

[0084] The point matching method based on epipolar line can effectively reduce the matching error caused by factors such as camera viewing angle and resolution. In traditional stereo vision point matching, the matching of points is often affected by noise, light changes and background complexity, resulting in inaccurate matching results. By using the point matching method based on epipolar line, the points can be constrained by using the geometric relationship of the cameras, greatly improving the accuracy of the matching, ensuring that the spatial coordinates obtained in the subsequent triangulation are more accurate, and improving the robustness of the overall positioning system.

[0085] As shown in Figure 3 , in step S310, the point matching method based on epipolar line is as follows:

[0086] In the epipolar geometry between the two cameras, the p1, p2 points on the imaging planes of the two cameras are the projections of the spatial point p, i.e. p1, p2 are a pair of matching points;

[0087] The epipolar lines l1, l2 are the intersection lines between the epipolar plane O1O2P and the two image planes I1, I2, and they are in a corresponding relationship, so that by determining the p1 point and the fundamental matrix F between the two cameras, the epipolar line l2 can be solved.

[0088] The fundamental matrix F describes the relationship between the two cameras, which is composed of the camera intrinsic parameter K and the camera extrinsic parameter t ^ R, which have been obtained in the camera pose calibration.

[0089] F = K -T t^RK -1 Equation (3)

[0090] l2=Fp1

[0091] Using the above formula, we can derive the equation for point p1 in camera one corresponding to the epipolar line l2 in camera two. If the camera's intrinsic and extrinsic parameters and infrared point detection accuracy are high enough, then point p2 in camera two will fall on the epipolar line l2, thus p1 and p2 can be regarded as matching points.

[0092] like Figure 4 The diagram illustrates the image point matching effect between two cameras at the same time stamp. A, B, and C represent three infrared LED light sources, and the straight lines in the diagram represent the mapping of the epipolar line in camera one to that in camera two. Points on the epipolar line of camera two correspond to their respective points in camera one.

[0093] Epipolar matching effectively reduces interference from noise or ambient light variations in traditional methods. Compared to matching methods based on pixel grayscale values ​​or colors, epipolar matching is less susceptible to changes in strong light or shadows in the environment. In complex environmental conditions, the signals from infrared LEDs are generally stable, and epipolar constraints further reduce errors introduced during the matching process due to background complexity or image noise.

[0094] In addition, step S300 also includes the following step: triangulation;

[0095] According to the definition of epipolar geometry, let x1 and x2 be the normalized coordinates of two feature points, then we have:

[0096] s1x1=s2Rx2+t Equation (4)

[0097] The rotation matrix R and translation vector t between the two cameras have been obtained through camera pose calibration, so the two depth values ​​s1 and s2 can be solved.

[0098]

[0099] After obtaining the actual depth information of the three infrared LEDs using triangulation, the spatial coordinates of these three infrared LEDs in the camera's coordinate system can be determined; and the centroid position of these three infrared LEDs can be used as the spatial position of the quadruped robot itself.

[0100] This method utilizes epipolar geometry constraints and calibrated camera parameters to solve for depth values, which can obtain more accurate spatial coordinates compared to direct triangulation, with positioning accuracy reaching the millimeter level.

[0101] Furthermore, step S400 includes the following steps:

[0102] Define three LED beads as ABC, with centroid D. The spatial coordinates of points A, B, and C are given as (A...). x A y A z (B) x B y B z ), (C x C y C z It can be achieved through spatial geometric relationships. Determine the spatial coordinates of the centroid D (D x D y D z ).

[0103] like Figure 5 As shown, this scheme reduces the impact of single-point measurement errors by calculating the center of gravity instead of using the position of a single LED. It utilizes the redundant information from multiple infrared LEDs to calculate the pose using a center of gravity algorithm, ensuring that valid position information can still be output even if some points are lost. Ultimately, the center of gravity of three LEDs can be used as the high-precision spatial coordinates of the quadruped robot.

[0104] The technical principles of the present invention have been described above with reference to specific embodiments. These descriptions are merely for explaining the principles of the invention and should not be construed as limiting the scope of protection of the invention in any way. Based on this explanation, those skilled in the art can readily conceive of other specific embodiments of the invention without inventive effort, and these embodiments will all fall within the scope of protection of the present invention.

Claims

1. An infrared-based quadruped robot positioning method, characterized by, The method comprises the following steps: S100, calibrate the internal and external parameters of the infrared camera, and solve the spatial pose of the camera; S200, capture the infrared light on the quadruped robot, and calculate the aperture centroid coordinates through edge detection; S300, use the matching conditions between the same light points in different camera images to obtain the spatial coordinates of the three light beads through triangulation; S400, calculate the position information of the quadruped robot in space by using the geometric relationship between the three light beads.

2. The infrared-based quadruped robot localization method of claim 1, wherein, In step S100: The internal parameters of the infrared camera include focal length, principal point coordinates and distortion coefficients; The external parameters of the infrared camera represent the pose of the camera in the world coordinate system; The internal parameters of the infrared camera are usually calibrated at the factory, and the external parameters of the infrared camera are calibrated through a checkerboard calibration board; The PnP method in the 2D-3D pose estimation method is used to solve the PnP problem by minimizing the re-projection error to solve the spatial pose of the camera.

3. The infrared-based quadruped robot localization method of claim 2, wherein, In step S100, the following steps are included: Two infrared cameras are used to capture the pixel coordinates u of the corners of the checkerboard by taking pictures of the checkerboard calibration board i and a checkerboard coordinate system is established based on the vertices of the checkerboard Since the size of each grid is known, the 3D coordinates P of each corner point in the checkerboard coordinate system can be determined i and the scale factor s, so as to obtain the 2D-3D corresponding points; The external parameters of the infrared camera are the transformation matrix T from the checkerboard coordinate system to the camera coordinate system, and the internal parameters of the infrared camera are K; the transformation matrix T is optimized by calculating the minimum re-projection error e through formula (1); The method can obtain the transformation matrix T of the two infrared cameras relative to the chessboard coordinate system 1c , T 2c ; In order to define the camera one as a world coordinate system, the extrinsic parameters of the camera one can be set as a unit matrix, i.e. The transformation matrix of the camera two relative to the camera one is 4. The infrared-based quadruped robot localization method of claim 3, wherein, In step S200, the infrared camera is used to capture the infrared light emitted by the three infrared light beads; S210, convert the RGB image captured by the camera into a grayscale image, and set a grayscale threshold for binary processing; S220, in the binary image, scan from the top left corner to find the first foreground pixel, and the pixel value of the foreground pixel is 255, which is a white pixel; Take the foreground pixel as the starting point, track along the foreground pixel boundary and record the boundary points; S230, after obtaining all the infrared light outlines, calculate the pixel centroid points in the outline area as the pixel coordinates of the infrared light beads, and the calculation formula is as follows: where R is the profile radius, x, y are the horizontal and vertical coordinates of the pixel coordinates, I (x,y) is the pixel value of the specified point, c x , c y is the pixel centroid coordinate value.

5. The infrared-based quadruped robot localization method of claim 4, wherein, In step S300, the following steps are included: Step S310, after extracting the infrared light centroid coordinates from the frame images of the two cameras, the actual spatial coordinates of the three light beads are calculated using the triangulation method, which requires corresponding matching of the three points of the two infrared cameras. The point matching method based on epipolar line is adopted to realize more accurate point matching by calculating the epipolar line corresponding to the point in camera one in camera two.

6. The infrared-based quadruped robot localization method of claim 5, wherein, In step S310, the point matching method based on epipolar line is as follows: In the epipolar geometry between the two cameras, p1 and p2 on the imaging planes of the two cameras are the projections of the spatial point p, i.e. p1 and p2 are a pair of matching points; The epipolar lines l1 and l2 are the intersection lines between the epipolar plane O1O2P and the two image planes I1 and I2, and they are in a corresponding relationship, so that the epipolar line l2 can be solved by determining the point p1 and the fundamental matrix F between the two cameras; The fundamental matrix F describes the relationship between two cameras, which is composed of the camera intrinsic K and the camera extrinsic t ^ Rconstitutes, which has been obtained in the camera pose calibration; F = K -T t = RK -1 Equation (3) l2=Fp1 Through the above formula, the equation of the epipolar line l2 corresponding to the point p1 in camera one in camera two can be derived; if the internal and external parameters of the camera and the infrared point detection accuracy are high enough, the point p2 in camera two will fall on the epipolar line l2, so that p1 and p2 can be regarded as matching points.

7. The infrared-based quadruped robot localization method of claim 6, wherein, Step S300 further comprises the following steps: According to the definition of polar geometry, let x1, x2 be the normalized coordinates of two feature points, then there is: s1x1=s2Rx2+t Equation (4) The rotation matrix R and translation vector t between the two cameras have been obtained through camera pose calibration, so the two depth values s1, s2 can be solved; After obtaining the actual depth information of the three infrared lamp beads by using the triangulation method, the spatial coordinates of the three infrared lamp beads in the camera one coordinate system can be determined respectively; and the center of gravity position of the three infrared lamp beads is used as the spatial position of the four-legged robot itself.

8. The infrared-based quadruped robot localization method of claim 7, wherein, The step S400 comprises the following steps: The step S400 comprises the following steps: Definition of three light beads as ABC, the gravity center as D, the spatial coordinates of the spatial points A, B, C are respectively (A x ,A y ,A z ), (B x ,B y ,B z ), (C x ,C y ,C z ), the spatial coordinates of the gravity center point D (D x ,D y ,D z ) can be determined through the spatial geometric relationship .

Images