Monocular vision positioning method suitable for biped robot

Abstract

The invention provides a monocular vision positioning method suitable for a biped robot, and the method comprises the steps: employing an AR Tag code as a visual marker, building a mapping relation between a pixel coordinate (ui, vi) of a central point of the AR Tag code and an actual distance di through nonlinear calibration, estimating the depth information from the central point of the AR Tag code to the robot according to the pixel coordinate, and carrying out the positioning of the robot. Therefore, three-dimensional coordinates of the AR Tag code in a robot camera coordinate system are obtained, and coordinates of the robot in a world coordinate system are obtained through coordinate system transformation. The defect that a monocular vision positioning system cannot acquire depth information is overcome, the positioning precision of the monocular vision positioning system is effectively improved, and the monocular vision positioning system has a wide application prospect in a positioning scene with low cost, low power consumption and low computing power.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of computer image processing and target positioning, and particularly relates to a monocular vision positioning method suitable for a biped robot. BACKGROUND

[0002] With the rapid development of biped robots, the autonomous positioning technology of the robot has become a key problem in the research of the robot, and the response speed and positioning accuracy of the autonomous positioning of the robot can greatly affect the accuracy and accuracy of navigation and decision-making. The traditional autonomous positioning method mainly relies on multi-view stereo vision and laser radar and other sensors, but is limited by the high cost of equipment, complex calculation and slow real-time response speed. In comparison, the monocular vision positioning system has the advantages of low cost, easy deployment and maintenance, but it faces many challenges in positioning accuracy. The monocular vision positioning system cannot directly obtain the depth information in the scene, and the feature point extraction and matching of adjacent image frames can only obtain the relative scale information, and it is difficult to accurately determine the three-dimensional position of the target object, and the accuracy in the actual application scene is limited.

[0003] The visual marker has the advantages of stable recognition, strong anti-environmental interference ability and fast recognition speed, and is commonly used in visual feedback control of robots or unmanned aerial vehicles. Since the pixel coordinates and the three-dimensional coordinates in the world are often not linearly mapped, it is difficult to obtain the accurate three-dimensional position information of the robot based on the recognition information of the visual marker. SUMMARY

[0004] To solve the above problems, the present application discloses a monocular vision positioning method suitable for a biped robot, which recognizes an AR Tag code pasted on the ground, and obtains the accurate three-dimensional position information of the AR Tag code through nonlinear calibration, to overcome the deficiency that the monocular vision positioning system cannot obtain depth information.

[0005] A monocular vision positioning method suitable for a biped robot, the biped robot recognizes an AR Tag code pasted on the ground through a monocular camera, and uses nonlinear calibration to establish the mapping relationship between the pixel coordinates of the center point of the AR Tag code and the actual distance, estimates the depth information of the center point of the AR Tag code to the robot according to the pixel coordinates, thereby obtaining the three-dimensional coordinates of the AR Tag code in the camera coordinate system of the robot, and then uses coordinate system transformation to obtain the coordinates of the robot in the world coordinate system.

[0006] Further, the AR Tag code is pasted on the ground, and its coordinates (X0, Y0, Z0) in the world coordinate system are fixed, the biped robot equipped with a monocular camera is placed directly behind the AR Tag code, the AR Tag code is recognized by the robot, and the id and rotation angle θ0 of the AR Tag code are obtained.

[0007] Further, the relative position between the biped robot and the AR Tag code is changed at intervals of 10 cm, the pixel coordinates (u i ,v i ) of the center point of the AR Tag code and the actual distance d i are recorded, and a calibration equation between the pixel coordinates of the center point of the AR Tag code and the actual distance is obtained:

[0008] d i =f(v i ), wherein v i is the vertical coordinate of the center point of the AR Tag code in the pixel coordinate system at the sampling point, and d i is the actual distance between the center point of the AR Tag code and the sampling point.

[0009] Further, when the robot is positioned, the AR Tag code corresponding to the id is identified, the pixel coordinates (u, v) of the center point and the rotation angle θ are obtained, and the depth of the AR Tag code in the camera is estimated according to the calibration equation:

[0010] Z c =f(v); wherein v is the vertical coordinate of the center point of the AR Tag code in the pixel coordinate system when the robot is positioned, and Z c is the depth of the AR Tag code in the camera.

[0011] Further, after the depth of the AR Tag code in the camera is obtained according to the calibration equation, the three-dimensional coordinates of the AR Tag code in the robot camera coordinate system are calculated according to the pinhole camera model:

[0012]

[0013] wherein (u, v) is the pixel coordinates of the center point of the AR Tag code when the robot is positioned, K is the camera intrinsic matrix, Z c is the depth of the AR Tag code in the camera, and (X c ,Y c ,Z c ) are the three-dimensional coordinates of the AR Tag code in the robot camera coordinate system.

[0014] Further, the three-dimensional coordinates of the AR Tag code in the robot coordinate system are calculated according to the transformation matrix from the camera coordinate system to the robot coordinate system:

[0015]

[0016] wherein is the transformation matrix from the camera coordinate system to the robot coordinate system, and (X r ,Y r ,Z r) is the three-dimensional coordinate of the AR Tag code in the robot coordinate system.

[0017] Further, the coordinate of the robot in the world coordinate system is calculated according to the coordinate of the AR Tag code in the world coordinate system:

[0018] Rotation matrix of the robot coordinate system to the world coordinate system

[0019] In the formula, θ is the rotation angle of the AR Tag code corresponding to id when the robot is positioned, and θ0 is the rotation angle of the AR Tag code corresponding to id when the robot is calibrated.

[0020] Translation matrix

[0021] In the formula, (X0, Y0, Z0) is the coordinate of the AR Tag code corresponding to id in the world coordinate system.

[0022] Transformation matrix of the robot coordinate system to the world coordinate system That is, the coordinate (X w ,Y w ,Z w ) of the robot in the world coordinate system can be obtained.

[0023] Advantages of the present application:

[0024] The present application establishes the mapping relationship between the pixel coordinates (u i ,v i ) of the center point of the AR Tag code and the actual distance d i , estimates the depth information of the center point of the AR Tag code to the robot according to the pixel coordinates, thereby obtaining the three-dimensional coordinates of the AR Tag code in the robot camera coordinate system, and then uses coordinate system transformation to obtain the coordinate of the robot in the world coordinate system; the present application solves the problem that the monocular vision positioning system cannot obtain depth information, and effectively improves the positioning accuracy of the monocular vision positioning system. BRIEF DESCRIPTION OF DRAWINGS

[0025] Figure 1 Step flow chart of the present application;

[0026] Figure 2 Robot positioning scene and coordinate system diagram of the present application. DETAILED DESCRIPTION

[0027] The present application will be further clarified by the following description and examples, which should be understood as describing particular embodiments and not limiting the scope of the application. It should be noted that the terms "front", "back", "left", "right", "upper" and "lower" used in the following description refer to directions in the drawings, and the terms "inner" and "outer" refer to directions towards or away from the geometric center of a particular component.

[0028] As shown in the following figure, a monocular vision positioning method suitable for a biped robot in this embodiment. It includes the following steps: Figure 1

[0029] S1: Monocular camera internal and external parameter calibration:

[0030] Zhang Zhengyou chessboard calibration method is adopted, and the internal and external parameters of the camera are calibrated by OpenCV to remove lens distortion, and the mapping relationship between the image coordinate system uOv and the camera coordinate system XcOYc is obtained as follows:

[0031] Where (c x ,c y ) is the intersection of the camera optical axis and the image plane in the uOv coordinate, fx and fy are the pixel focal lengths of each pixel in the Xc and Yc directions;

[0032] The camera intrinsic matrix K is obtained by camera internal and external parameter calibration, and the original image is corrected according to the external parameter.

[0033] S2: AR Tag code recognition:

[0034] AR Tag code is pasted on the ground, and its coordinates (X0, Y0, Z0) in the world coordinate system are fixed;

[0035] Place the biped robot with a monocular camera behind the AR Tag code to ensure that the AR Tag code is in front of the biped robot;

[0036] Call the aruco library in OpenCV to realize AR Tag code recognition, and obtain the id, center pixel coordinates (u i ,v i ), and rotation angle θ0 of the corresponding AR Tag code;

[0037] S3: AR Tag code vision calibration:

[0038] ​The monocular vision positioning system has the disadvantage of being unable to obtain the depth information of the target object, and the error of directly estimating the depth information according to the size of the object is large, and is greatly affected by the posture of the object. The AR Tag code has the advantages of stable identification, strong anti-environmental interference ability and fast identification speed, and is pasted on the ground to fix its world coordinates and posture, thereby reducing the influence of external environmental changes on positioning. The purpose of calibration is to obtain the mapping relationship between the pixel coordinates of the center point of the AR Tag code and the actual distance, so as to estimate the depth information according to the pixel coordinates of the center point of the AR Tag code.

[0039] The AR Tag code is placed in front of the biped robot and within the camera field of view, the robot is moved at an interval of 10 cm, and the pixel coordinates (u i ,v i ) of the AR Tag code and the actual distance d i between the robot and the AR Tag code are recorded.

[0040] Taking the pixel longitudinal coordinate v i of the AR Tag code as the X-axis data and the actual distance d i between the robot and the AR Tag code as the Y-axis data, the Curve Fitting curve fitting tool in MATLAB is used to calculate the calibration equation d i =f(v i ) between the pixel coordinates (u i ,v i ) of the center point of the AR Tag code and the actual distance d i .

[0041] S4: AR Tag code three-dimensional coordinate estimation:

[0042] When the robot is positioned, the id, center point pixel coordinates (u, v) and rotation angle θ of the corresponding AR Tag code are identified, the depth Z c between the AR Tag code and the robot is estimated according to the calibration equation d i =f(v i ), and Z c =f(v).

[0043] The pinhole camera model can describe the transformation relationship between the pixel coordinate system and the camera coordinate system in the monocular camera, so as to calculate the three-dimensional coordinates (X c ,Y c ,Z c ) of the AR Tag code in the robot camera coordinate system:

[0044]

[0045] S5: Robot three-dimensional coordinate estimation:

[0046] World coordinate system O-x w y w z w Robot coordinate system O-x r y r z r Camera coordinate system O-x c y c z c The schematic diagram of the AR Tag code in the world coordinate system is shown in Figure 2

[0047] The installation position of the monocular camera in the robot is fixed, and the transformation matrix of the camera coordinate system to the robot coordinate system can be calculated According to the transformation matrix The three-dimensional coordinates (X r ,Y r ,Z r ) of the AR Tag code in the robot coordinate system can be calculated:

[0048]

[0049] The rotation angle of the robot coordinate system relative to the world coordinate system is θ0-θ, and the rotation matrix of the robot coordinate system to the world coordinate system is

[0050] The translation matrix The transformation matrix of the robot coordinate system to the world coordinate system

[0051] The coordinates of the robot in the world coordinate system are (X w ,Y w ,Z w ):

[0052]

[0053] The embodiment is to identify the visual marker (AR Tag code), first measure to establish the calibration equation, then estimate the depth according to the calibration equation, and finally obtain the positioning information by coordinate transformation, which improves the accuracy.

[0054] The technical means disclosed in the present application is not limited to the technical means disclosed in the above embodiment, but also includes the technical solutions composed of any combination of the above technical features.​

Claims

1. A monocular vision localization method suitable for bipedal robots, comprising the following steps: S1. Perform intrinsic and extrinsic parameter calibration on the monocular camera to determine the image coordinate system uOv and the world coordinate system X. w OY w The mapping relationship between the parameters is used to obtain the camera intrinsic parameter matrix K and extrinsic parameters, and distortion correction is performed on the original image based on the extrinsic parameters. S2. Affix the AR Tag code to the ground and fix its world coordinates in the world coordinate system. A bipedal robot equipped with a monocular camera is placed directly behind an AR tag. The robot then identifies the AR tag to obtain its ID and rotation angle. ; S3. Change the relative position between the bipedal robot and the AR Tag code, record the pixel coordinates of the center point of the AR Tag code and the actual distance, and obtain the calibration equation between the pixel coordinates of the center point of the AR Tag code and the actual distance. S4. During robot localization, based on the camera intrinsic parameter matrix K obtained in step S1, the AR Tag ID and rotation angle obtained in step S2... The pixel coordinates of the center point of the AR Tag code obtained in step S3 ( (and actual distance) Calibration equations between Calculate the coordinates of the AR Tag code in the robot's camera coordinate system; S5. Based on the world coordinates of the AR Tag code obtained in step S2 ( ), rotation angle The rotation angle of the AR Tag code obtained in step S4 3D coordinates in the robot camera coordinate system The coordinates of the AR Tag code in the robot coordinate system are obtained, and the coordinates of the robot in the world coordinate system are obtained according to the transformation relationship between the robot coordinate system and the AR Tag code coordinates.

2. A monocular vision localization method for bipedal robots according to claim 1, comprising the following steps: The specific steps of step 3 are as follows: S301. Keep the AR Tag code placed directly in front of the bipedal robot and within the camera's field of view. Move the robot at 10cm intervals and record the pixel coordinates of the AR Tag code. (and the actual distance between the robot and the AR tag) ; S302, using the pixel coordinates of the AR Tag code For X-axis data, the actual distance between the robot and the AR tag. For Y-axis data, the pixel coordinates of the AR tag center point are calculated using the Curve Fitting tool in MATLAB. (and actual distance) Calibration equations between In the formula The vertical coordinate of the AR Tag center point in the pixel coordinate system is given by the given location. This represents the actual distance between the center point of the AR Tag code and the sampling point.

3. A monocular vision localization method for bipedal robots according to claim 1, comprising the following steps: Step 4 specifically includes the following: S401. Recognize the AR Tag code corresponding to the ID and obtain the center point pixel coordinates. ), rotation angle The depth of the AR tag in the camera is estimated based on the calibration equation. In the formula The vertical coordinate of the center point of the AR tag in the pixel coordinate system during robot localization. The depth of the AR tag in the camera; S402. After obtaining the depth of the AR Tag in the camera according to the calibration equation, calculate the three-dimensional coordinates of the AR Tag in the robot camera coordinate system using the pinhole camera model. ): In the formula ( () represents the pixel coordinates of the center point of the AR Tag code during robot localization. For the camera intrinsic parameter matrix, The depth of the AR tag in the camera, ( () represents the three-dimensional coordinates of the AR Tag code in the robot's camera coordinate system.

4. A monocular vision localization method for bipedal robots according to claim 1, comprising the following steps: Step 5 specifically includes the following steps: Step 501: The transformation matrix from the camera coordinate system to the robot coordinate system. Calculate the three-dimensional coordinates of the AR Tag code in the robot coordinate system: In the formula Let be the transformation matrix from the camera coordinate system to the robot coordinate system. () represents the three-dimensional coordinates of the AR Tag code in the robot coordinate system; Step 502: Calculate the robot's coordinates in the world coordinate system based on the AR Tag's coordinates in the world coordinate system: rotation matrix from robot coordinate system to world coordinate system. ; In the formula The rotation angle of the AR tag code corresponding to the robot's ID during localization. The rotation angle of the AR Tag code corresponding to the ID at the time of calibration; Translation matrix In the formula ( () represents the coordinates of the corresponding AR Tag code in the world coordinate system; Transformation matrix from robot coordinate system to world coordinate system ,according to The robot's coordinates in the world coordinate system can then be determined. ).

Images