Mobile robot tail end large scene follow-up guiding method based on monocular vision

Abstract

The invention belongs to the field of robot positioning, and discloses a mobile robot tail end large scene follow-up guiding method based on monocular vision, which comprises the following steps of: firstly, obtaining high-precision distance information based on measurement of a laser tracker, and embedding the high-precision distance information as a constraint into a mainstream light beam adjustment calculation model to ensure high-precision offline global calibration of a series of positioning points in a large range; then, on the basis of calibration information, a self-adaptive weighted camera pose high-robustness iterative optimization method is provided, a monocular camera global pose solving model is established, and the problem that the camera pose solving precision is low is solved; and finally, the global pose information of the monocular camera is combined, and the global pose of the tail end of the robot is inversely calculated based on the conversion relation between the coordinate systems. The method solves the problem that the binocular vision system of the mobile industrial robot is difficult to autonomously position to the to-be-measured area in a large scene, gives consideration to the measurement range, the guide precision and the robustness, and is suitable for autonomous follow-up guide positioning of the mobile robot in the large scene.

Description

Technical Field

[0001] The invention belongs to the field of robot positioning and relates to a large-scene follow-up guidance method for an industrial robot terminal based on monocular vision. Background Art

[0002] Mobile industrial robots are widely used in the processing and assembly of large components due to their large operating range, high efficiency, and good flexibility.

[0003] The positioning accuracy of the binocular vision system depends on the field of view. The positioning accuracy will be very high only when the area to be measured is in the center of the field of view. However, the manufacturing site of large components is large, so it is difficult to directly search the area to be measured by the binocular vision system at the end of the mobile robot. Even the target to be measured directly exceeds the measurement range of the binocular vision, resulting in the failure of the binocular vision system to position, and thus the positioning accuracy of the mobile robot cannot be guaranteed. In order to guide the binocular vision system to the area to be measured to complete its positioning task, an additional positioning system is still required to assist it in completing the positioning task. The traditional positioning method of mobile robots in a large range that relies on laser trackers is often difficult to complete the guidance and positioning task due to the complex manufacturing site environment and severe line of sight obstruction, and the cost is very high. The positioning method assisted by monocular vision guidance provides a solution to the problem that the binocular vision system is difficult to autonomously search for the area to be measured.

[0004] The patent "A camera robot terminal posture positioning system and method", patent number CN202311270029.7, discloses a tracking and positioning method for the position and orientation of the robot terminal based on binocular reconstruction of three-dimensional point cloud computing coordinates. The patent "Robot and positioning method with visual positioning function", patent CN202111238007.3, discloses a robot positioning method based on image processing and multi-sensor data analysis. The above two methods realize the positioning of the robot through binocular reconstruction of point cloud computing coordinates and image processing data analysis, but during the operation of mobile robots in large scenes, they still face the problem that the binocular vision system is difficult to complete the positioning task by itself. Therefore, the present invention proposes a large-scene follow-up guidance method for the terminal of a mobile robot based on monocular vision to realize autonomous guidance and positioning of the terminal of a mobile robot in a large scene. Summary of the invention

[0005] This invention is aimed at the operation scene of mobile industrial robots in large scenes. In order to solve the problem that the binocular vision system is difficult to autonomously search the area to be measured, a large-scene follow-up guidance method for the terminal of an industrial robot based on monocular vision is invented. First, high-precision distance information is obtained based on laser tracker measurement, and it is embedded as a constraint in the mainstream bundle adjustment calculation model, ensuring high-precision offline global calibration of a series of positioning points in a large range; then, based on the calibration information, an adaptive weighted camera pose high-robust iterative optimization method is proposed, and a monocular camera global pose solution model is established to solve the problem of low camera pose solution accuracy; finally, combined with the global pose information of the monocular camera, the global pose of the robot terminal is inversely calculated based on the conversion relationship between each coordinate system. This method solves the problem that the binocular vision system of a mobile industrial robot in a large scene is difficult to autonomously search the area to be measured, taking into account the measurement range, guidance accuracy and robustness, and is suitable for autonomous follow-up guidance and positioning of mobile robots in large scenes, with good versatility and promotion.

[0006] The technical solution of the present invention:

[0007] A large scene following guidance method for a mobile robot terminal based on monocular vision, the steps are as follows:

[0008] The first step is to accurately measure the global positioning point with distance constraints.

[0009] First, in order to realize the follow-up guidance of the robot end by monocular vision in the large-scale component manufacturing site in a large scene, the expression of the collinearity condition equation is derived according to the perspective imaging principle of the monocular vision camera:

[0010]

[0011] Among them, (x, y) is the coordinate of the image point; (x0, y0) is the coordinate of the camera principal point; f is the camera principal distance; (X, Y, Z) is the coordinate of the measured point; (X S ,Y S ,Z S ) is the coordinate of the projection center; a i ,b i ,c i (i=1,2,3) is the rotation angle ω between the image space coordinate system and the image plane coordinate system around the x, y, and z axes, The trigonometric functions of κ;

[0012] Then, taking into account the image point error and linearizing the collinearity condition equation, the image point coordinate error equation is obtained:

[0013] V=A1X1+A2X2+A3X3-L (2)

[0014] Where L is the constant vector matrix, V is the image point coordinate residual matrix, X1, X2, and X3 are the measured point coordinate correction parameter matrix, the photogrammetric camera exterior orientation element correction matrix, and the camera intrinsic parameter correction matrix, respectively. A1, A2, and A3 are the coefficient matrices corresponding to X1, X2, and X3, respectively.

[0015] Set up multiple cooperative target points and use a laser tracker to measure and obtain the coordinates of each cooperative target point:

[0016]

[0017] Among them, (x Ti y Ti z Ti ) is the coordinate of the ith cooperative target point, and m is the number of cooperative target points;

[0018] In order to achieve high-precision offline global calibration of a series of positioning points in a large range, a set of high-precision distance constraints are added to the traditional bundle adjustment method. Based on the coordinates of the cooperative target points in formula (3), the distance constraint function F is constructed. ij :

[0019]

[0020] Among them, S i,j is the distance between the cooperating target points measured by the laser tracker, (X i Y i Z i )、(X j Y j Z j ) are the coordinates of each positioning point measured by the monocular camera;

[0021] Linearize equation (4), and its first-order Taylor expansion is:

[0022]

[0023] in, is the approximate initial value of the function; let ΔL i,j is the observed value S i,j The correction number is used to perform partial differential differentiation of the differential term to obtain the error equation of the distance constraint:

[0024]

[0025] in, t i,j and L i,j The expression is as follows:

[0026]

[0027] The internal parameters of the camera have been calibrated, so the bundle adjustment only regards the values ​​in X1 and X2 as unknowns. Then, the normal equations of the multi-tenant distance constraint and the error equation are arranged and combined to obtain the bundle adjustment model with additional distance constraint, as shown in formula (8):

[0028]

[0029] Among them, A L is the distance relative control coefficient matrix, L L is the constant vector matrix corresponding to the distance control;

[0030] Finally, the coordinates of each positioning point in the global coordinate system and the correction number of the camera's extrinsic parameters are iteratively solved, and then the positioning point coordinates and the camera's extrinsic parameters are corrected, and the corresponding coefficient matrix is ​​calculated to enter the next iteration until the correction number is less than the convergence threshold;

[0031] The second step is to accurately calculate the spatial pose of the monocular camera based on adaptive weighting

[0032] First, in order to reduce the influence of gross errors on the inverse accuracy of the camera spatial pose during online measurement, the improved IGG3 weight function is used to calculate the credibility of each point. As shown in formula (9):

[0033]

[0034] Among them, k0 = 1.0, k1 = 3.0, is the standardized error;

[0035] Furthermore, based on the perspective imaging model of the camera and the principle that the projection center, object point and corresponding image point are collinear, the objective function with weight coefficients is obtained:

[0036]

[0037] Where I is the identity matrix, R and t are the rotation matrix and translation vector to be solved, ω i is the weight coefficient of the i-th point, n is the number of object points and corresponding image points, p i is the three-dimensional coordinate of the object point, is the projection matrix, and its calculation expression is as follows:

[0038]

[0039] in, is the image point coordinates projected onto the normalized image plane by the object point, in pixels;

[0040] Then, the k-th iteration result of the translation vector is calculated by equation (12):

[0041]

[0042] The initial value of the iteration R (0) Given by the SRPnP algorithm, solving t (k) After that, the next iteration value of R is R (k+1) Calculated by formula (13):

[0043]

[0044] in is the kth iteration of the line of sight projection vector, calculated using formula (14):

[0045]

[0046] Finally, the singular value decomposition method is used to solve R (k+1) and t (k+1) , until the iteration convergence condition is met, the iteration is terminated, and the calculated transformation relationship is the transformation parameter of the camera coordinate system relative to the global coordinate system and

[0047] Step 3: Mobile robot terminal guidance and positioning in large scenes

[0048] First, a rigid connection is formed between the monocular camera and the robot end control point. Assuming that the calibration relationship between the two is known, the calibration parameters are: and After the robot end moves to the processing position, the camera marks the camera through the camera, and then the conversion parameters of the camera coordinate system relative to the global coordinate system when moving to the i-th processing position are calculated by the second step: and The transformation relationship between the end control point coordinate system and the global coordinate system when moving to the i-th processing position is and for:

[0049]

[0050] Then, according to the rigid body posture transformation principle, the transformation matrix parameters of the control point coordinate system relative to the global coordinate system are calculated. and

[0051]

[0052] in, The three parameters represent the coordinates of the origin of the end control point coordinate system in the global coordinate system when moving to the i-th processing position, that is, the position of the robot end in the global coordinate system (X Mi ,Y Mi ,Z Mi ) T ; is the rotation matrix, Obtain the three rotation angles of the robot end relative to each coordinate axis of the global coordinate system Finally, the spatial pose parameters of the robot terminal are solved.

[0053] Beneficial effects of the present invention: The present invention proposes a guidance and positioning idea of ​​offline calibration of global positioning points - monocular online following measurement - reverse calculation of camera pose, and adopts the above idea to realize autonomous following positioning of mobile robots in large scenes, taking into account the measurement range, guidance accuracy and robustness, and also ensuring the accuracy of global information calibration and camera pose solution. It is suitable for online following positioning of mobile robots in large-scale component processing and manufacturing sites in large scenes, and has broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 This is a flow chart of the large-scene following guidance method for the terminal of a mobile robot based on monocular vision.

[0055] Figure 2 This is a schematic diagram of the terminal guidance and positioning of the mobile robot.

[0056] In the figure: 1-mobile robot; 2-monocular camera; 3-laser tracker; 4-cooperative target; 5-positioning point; G -X G Y G Z G is the global coordinate system, O C -X C Y C Z C is the camera coordinate system, O R -X R Y R Z R is the coordinate system of the end control point, O L -X L Y L Z L is the laser tracker coordinate system. DETAILED DESCRIPTION

[0057] The specific implementation of the present invention is described in detail below in combination with the technical scheme and the accompanying drawings.

[0058] This embodiment uses a scaled-down simplified thin-walled cabin model to simulate the local positioning scene of a large component, including 16 positioning points and 4 cooperative target points.

[0059] The first step is to accurately measure the global positioning point with distance constraints.

[0060] First, set up 4 cooperative targets and 10 positioning points, and use a laser tracker to measure the coordinates of the cooperative targets T LT :

[0061]

[0062] Then, a monocular camera is used to measure each positioning point, and the bundle adjustment model with additional distance constraints is constructed according to formula (8) in combination with the coordinates of the above cooperative target points.

[0063] Then, based on the above model, the coordinates of each positioning point in the global coordinate system and the correction number of the camera extrinsic parameters are iteratively solved, and then the coordinates of the positioning point and the camera extrinsic parameters are corrected, and the corresponding coefficient matrix is ​​calculated to enter the next iteration until the correction number is less than the convergence threshold, and the coordinates S of all positioning points and cooperative target points in the global coordinate system are solved. G and T G :

[0064]

[0065]

[0066] The second step is to accurately calculate the spatial pose of the monocular camera based on adaptive weighting

[0067] According to formula (10), an adaptive weighted monocular camera global pose solution model is constructed. Based on formulas (12) and (13), the transformation relationship between the camera coordinate system and the global coordinate system is solved by iterative optimization, that is, the monocular camera global pose:

[0068]

[0069] Step 3: Mobile robot terminal guidance and positioning in large scenes

[0070] First, the monocular camera is fixed to the robot end, and the conversion relationship between the two coordinate systems is known. Then the robot end moves to the four processing positions, and the camera marks are photographed by the camera. The relationship between the current camera coordinate system and the global coordinate system is obtained by the method in the second step. Finally, the robot end position at each processing position in the global coordinate system is calculated according to formula (16):

[0071]

[0072]

[0073] The present invention proposes a guidance and positioning idea of ​​offline calibration of global positioning points - monocular online following measurement - reverse calculation of camera pose. The above idea is adopted to realize autonomous following positioning of mobile robots in large scenes, taking into account the measurement range, guidance accuracy and robustness, and also ensuring the accuracy of global information calibration and camera pose solution. It is suitable for online following positioning of mobile robots in large-scale component processing and manufacturing sites, and has broad application prospects.

Claims

1. A large-scene follow-up guidance method for a mobile robot terminal based on monocular vision, characterized in that: Here are the steps: The first step is to accurately measure the global positioning point with distance constraints. First, in order to realize the follow-up guidance of the robot end by monocular vision in the large-scale component manufacturing site in a large scene, the expression of the collinearity condition equation is derived according to the perspective imaging principle of the monocular vision camera: Among them, (x, y) is the coordinate of the image point; (x0, y0) is the coordinate of the camera principal point; f is the camera principal distance; (X, Y, Z) is the coordinate of the measured point; (X S ,Y S ,Z S ) are the coordinates of the projection center; ai, bi, ci (i = 1, 2, 3) are the rotation angles around the x, y, and z axes between the image space coordinate system and the image plane coordinate system The trigonometric functions formed; Then, taking into account the image point error and linearizing the collinearity condition equation, the image point coordinate error equation is obtained: V=A1X1+A2X2+A3X3-L (2) Where L is the constant vector matrix, V is the image point coordinate residual matrix, X1, X2, and X3 are the measured point coordinate correction parameter matrix, the photogrammetric camera exterior orientation element correction matrix, and the camera intrinsic parameter correction matrix, respectively. A1, A2, and A3 are the coefficient matrices corresponding to X1, X2, and X3, respectively. Set up multiple cooperative target points and use a laser tracker to measure and obtain the coordinates of each cooperative target point: Among them, (x Ti y Ti z Ti ) is the coordinate of the ith cooperative target point, and m is the number of cooperative target points; In order to achieve high-precision offline global calibration of a series of positioning points in a large range, a set of high-precision distance constraints are added to the traditional bundle adjustment method. Based on the coordinates of the cooperative target points in formula (3), the distance constraint function F is constructed. ij : F i,j =S i,j 2 -(X i -X j ) 2 -(Y i -Y j ) 2 -(Z i -Z j ) 2 (4) Among them, S i,j is the distance between the cooperating target points measured by the laser tracker, (X i Y i Z i )、(X j Y j Z j ) are the coordinates of each positioning point measured by the monocular camera; Linearize equation (4), and its first-order Taylor expansion is: in, is the approximate initial value of the function; let ΔL i,j is the observed value S i,j The correction number is used to perform partial differential differentiation of the differential term to obtain the error equation of the distance constraint: ΔL i,j =A Li,j t i,j -L i,j (6) Among them, A Li,j ,t i,j and L i,j The expression is as follows: The internal parameters of the camera have been calibrated, so the bundle adjustment only regards the values ​​in X1 and X2 as unknowns. Then, the normal equations of the multi-tenant distance constraint and the error equation are arranged and combined to obtain the bundle adjustment model with additional distance constraint, as shown in formula (8): Among them, A L is the distance relative control coefficient matrix, L L is the constant vector matrix corresponding to the distance control; Finally, the coordinates of each positioning point in the global coordinate system and the correction number of the camera's extrinsic parameters are iteratively solved, and then the positioning point coordinates and the camera's extrinsic parameters are corrected, and the corresponding coefficient matrix is ​​calculated to enter the next iteration until the correction number is less than the convergence threshold; The second step is to accurately calculate the spatial pose of the monocular camera based on adaptive weighting First, in order to reduce the influence of gross errors on the inverse accuracy of the camera spatial pose during online measurement, the improved IGG3 weight function is used to calculate the credibility of each point. As shown in formula (9): Among them, k0 = 1.0, k1 = 3.0, is the standardized error; Furthermore, based on the perspective imaging model of the camera and the principle that the projection center, object point and corresponding image point are collinear, the objective function with weight coefficients is obtained: Where I is the identity matrix, R and t are the rotation matrix and translation vector to be solved, ω i is the weight coefficient of the i-th point, n is the number of object points and corresponding image points, p i is the three-dimensional coordinate of the object point, is the projection matrix, and its calculation expression is as follows: in, is the image point coordinates projected onto the normalized image plane by the object point, in pixels; Then, the k-th iteration result of the translation vector is calculated by equation (12): The initial value of the iteration R (0) Given by the SRPnP algorithm, solving t (k) After that, the next iteration value of R is R (k+1) Calculated by formula (13): in is the kth iteration of the line of sight projection vector, calculated using formula (14): Finally, the singular value decomposition method is used to solve R (k+1) and t (k+1) , until the iteration convergence condition is met, the iteration is terminated, and the calculated transformation relationship is the transformation parameter of the camera coordinate system relative to the global coordinate system and Step 3: Mobile robot terminal guidance and positioning in large scenes First, a rigid connection is formed between the monocular camera and the robot end control point. Assuming that the calibration relationship between the two is known, the calibration parameters are: and After the robot end moves to the processing position, the camera marks the camera through the camera, and then the conversion parameters of the camera coordinate system relative to the global coordinate system when moving to the i-th processing position are calculated by the second step: and The transformation relationship between the end control point coordinate system and the global coordinate system when moving to the i-th processing position is and for: Then, according to the rigid body posture transformation principle, the transformation matrix parameters of the control point coordinate system relative to the global coordinate system are calculated. and in, The three parameters represent the coordinates of the origin of the end control point coordinate system in the global coordinate system when moving to the i-th processing position, that is, the position of the robot end in the global coordinate system (X Mi ,Y Mi ,Z Mi ) T ; is the rotation matrix, Obtain the three rotation angles of the robot end relative to each coordinate axis of the global coordinate system Finally, the spatial pose parameters of the robot terminal are solved.

Images