Mobile robot positioning method and device, electronic equipment and storage medium

Abstract

The invention discloses a mobile robot positioning method and device, electronic equipment and a storage medium, which are used for solving the technical problem that under-constraint judgment and adaptive adjustment cannot be carried out on existing mobile robot positioning. The method comprises the following steps: acquiring first radar point cloud information of a laser radar in a world coordinate system; establishing a plurality of fitting planes according to the first radar point cloud information, and obtaining the normal of each fitting plane; calculating a minimum angle axis between the coordinate system of the mobile robot and each normal; calculating an effective observation proportion of the fitting plane corresponding to each minimum angle axis in all the fitting planes; determining an under-constraint direction of the mobile robot according to the effective observation proportion; and adjusting the noise in the under-constraint direction, and positioning the mobile robot according to the adjusted noise of each axis of the coordinate system of the mobile robot.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a mobile robot positioning method, device, electronic equipment and storage medium. Background Art

[0002] During localization, when a mobile robot is in an environment with few directional features, such as a sparsely treed country road, a narrow passage between residential buildings, or a small corridor next to an elevated highway, the lack of directional features can cause the mobile robot to drift. It is crucial to determine whether the current environment is underconstrained and how to adaptively and dynamically adjust the radar observation threshold to ensure robust localization in such environments, without the aid of other sensors and using only radar observation information.

[0003] The existing technology provides a method for laser radar point cloud positioning in a degraded environment based on feature point enhancement, which specifically includes: S1, data acquisition and data preprocessing: laser radar point cloud data acquisition and acceleration, angular velocity and direction information of IMU measurement equipment, complete data synchronization, compensate for motion distortion of laser radar point cloud data, and provide initial pose estimation for point cloud feature matching; S2, adaptive point cloud feature extraction in a degraded environment: comprehensively consider local geometric information and global distribution characteristics, dynamically adjust the threshold and weight distribution strategy of feature point extraction, and when there are fewer significant features in the tunnel, according to Rely on structural features; when there are significant features locally, local features are emphasized; S3, construct pseudo point cloud to enhance point cloud features: in view of the insufficient point cloud features in the tunnel environment, point cloud edge point prediction is performed to construct pseudo point cloud to enhance point cloud features; S4, integrate IMU pre-integration pose matrix for feature matching: select key frames and obtain pose matrix through IMU pre-integration to provide initial pose estimation for point cloud alignment; S5, introduce factor graph optimization algorithm in the back end: odometry factor between frames, global constraint factor between frames and global map, IMU pre-integration factor, to optimize the point cloud map.

[0004] However, the above method requires the use of other sensors, and when performing feature matching, the focus is on which feature points to match, without analyzing the scene. It cannot adapt to different scenes, resulting in low positioning accuracy. Summary of the Invention

[0005] The present invention provides a mobile robot positioning method, device, electronic equipment and storage medium, which are used to solve the technical problem that the existing mobile robot positioning cannot perform under-constraint judgment and adaptive adjustment.

[0006] The present invention provides a positioning method for a mobile robot, wherein the mobile robot is provided with a laser radar; the method comprises:

[0007] Get the first radar point cloud information of the laser radar in the world coordinate system;

[0008] Establishing a plurality of fitting planes according to the first radar point cloud information, and obtaining a normal of each of the fitting planes;

[0009] Calculating the minimum angle axis between the mobile robot coordinate system and each of the normal lines;

[0010] Calculate the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes;

[0011] Determining an underconstrained direction of the mobile robot according to the effective observation ratio;

[0012] The noise in the underconstrained direction is adjusted, and the mobile robot is positioned according to the adjusted noise of each axis of the mobile robot coordinate system.

[0013] Optionally, the step of obtaining first radar point cloud information of the laser radar in the world coordinate system includes:

[0014] Obtain the real-time position and posture of the mobile robot in the world coordinate system;

[0015] Obtain the second radar point cloud information collected by the laser radar in the mobile robot coordinate system;

[0016] The second radar point cloud information is converted into a world coordinate system according to the real-time pose to obtain the first radar point cloud information.

[0017] Optionally, the step of calculating the minimum angle axis between the mobile robot coordinate system and each normal line includes:

[0018] Calculate the angles between each axis of the mobile robot coordinate system and the normals of each fitting plane;

[0019] The minimum angle axis of each fitting plane is determined according to the included angle.

[0020] Optionally, the first radar point cloud information includes multiple radar points; and the step of establishing multiple fitting planes based on the first radar point cloud information and obtaining the normal of each of the fitting planes includes:

[0021] Search for multiple nearby points closest to each radar point in sequence;

[0022] Calculate the average coordinates of the plurality of adjacent points corresponding to each radar point to obtain the centroid coordinates;

[0023] Subtract the centroid coordinates from the coordinates of each adjacent point to obtain a covariance matrix;

[0024] Calculate three eigenvalues ​​and the eigenvectors corresponding to each eigenvalue according to the covariance matrix;

[0025] Determine whether the minimum eigenvalue is less than 0.001;

[0026] If so, the eigenvector corresponding to the minimum eigenvalue is used as a normal vector, the centroid coordinates and the normal vector are used to generate a fitting plane, and the normal vector is determined as a normal line of the fitting plane.

[0027] Optionally, the step of determining the underconstrained direction of the mobile robot according to the effective observation ratio includes:

[0028] Determine whether the proportion of each of the valid observations is less than a preset proportion threshold;

[0029] If so, the direction of the minimum angle axis corresponding to the effective observation ratio is determined to be the under-constrained direction.

[0030] Optionally, the step of adjusting the noise in the underconstrained direction and positioning the mobile robot according to the adjusted noise of each axis of the mobile robot coordinate system includes:

[0031] Get the ideal effective observation ratio;

[0032] Calculating the ratio of the effective observation ratio to the ideal effective observation ratio;

[0033] Obtaining effective observation noise in the underconstrained direction;

[0034] Calculating optimized noise according to the effective observation noise and the ratio;

[0035] constructing a Hessian matrix based on the optimized noise;

[0036] The mobile robot is positioned according to the Hessian matrix.

[0037] The present invention also provides a mobile robot positioning device, wherein the mobile robot is provided with a laser radar; the device comprises:

[0038] A first radar point cloud information acquisition module is used to obtain the first radar point cloud information of the laser radar in the world coordinate system;

[0039] a fitting module, configured to establish a plurality of fitting planes according to the first radar point cloud information, and obtain a normal of each of the fitting planes;

[0040] A minimum angle axis calculation module, used to calculate the minimum angle axis between the mobile robot coordinate system and each normal line;

[0041] The effective observation ratio calculation module is used to calculate the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes;

[0042] an underconstrained direction determination module, configured to determine the underconstrained direction of the mobile robot according to the effective observation ratio;

[0043] The positioning module is used to adjust the noise in the underconstrained direction and perform positioning of the mobile robot according to the adjusted noise of each axis of the mobile robot coordinate system.

[0044] Optionally, the first radar point cloud information acquisition module includes:

[0045] The real-time pose acquisition submodule is used to obtain the real-time pose of the mobile robot in the world coordinate system;

[0046] The second radar point cloud information acquisition submodule is used to obtain the second radar point cloud information collected by the laser radar in the mobile robot coordinate system;

[0047] The first radar point cloud information acquisition submodule is used to convert the second radar point cloud information into a world coordinate system according to the real-time posture to obtain the first radar point cloud information.

[0048] The present invention further provides an electronic device, comprising a processor and a memory:

[0049] The memory is used to store program code and transmit the program code to the processor;

[0050] The processor is configured to execute any one of the above mobile robot positioning methods according to instructions in the program code.

[0051] The present invention also provides a computer-readable storage medium, wherein the computer-readable storage medium is used to store program code, and the program code is used to execute the mobile robot positioning method as described in any one of the above items.

[0052] It can be seen from the above technical solution that the present invention has the following advantages: the present invention provides a mobile robot positioning method, and specifically discloses: obtaining first radar point cloud information of a laser radar in a world coordinate system; establishing several fitting planes based on the first radar point cloud information, and obtaining the normal of each fitting plane; calculating the minimum angle axis between the mobile robot coordinate system and each normal; calculating the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes; determining the under-constrained direction of the mobile robot based on the effective observation ratio; adjusting the noise of the under-constrained direction, and positioning the mobile robot based on the adjusted noise of each axis of the mobile robot coordinate system.

[0053] The present invention calculates the various fitting planes observed by the lidar, then calculates the minimum angle axis between the mobile robot coordinate system and each fitting plane, and judges the under-constrained direction of the mobile robot according to the effective observation ratio of each minimum angle axis, thereby achieving under-constrained judgment of the mobile robot observation and improving the positioning accuracy of the mobile robot by adaptively adjusting the threshold of the under-constrained direction. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0055] Figure 1 A flowchart of a mobile robot positioning method provided by an embodiment of the present invention;

[0056] Figure 2 A flowchart of a mobile robot positioning method provided by another embodiment of the present invention;

[0057] Figure 3 is a side view of the mobile robot;

[0058] Figure 4 is a top view of the mobile robot;

[0059] Figure 5 This is a structural block diagram of a mobile robot positioning device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0060] Embodiments of the present invention provide a mobile robot positioning method, device, electronic device and storage medium, which are used to solve the technical problem that the existing mobile robot positioning cannot perform under-constraint judgment and adaptive adjustment.

[0061] In order to make the purpose, features, and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described below 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 making creative work are within the scope of protection of the present invention.

[0062] See also Figure 1 , Figure 1 A flowchart of the steps of a mobile robot positioning method provided by an embodiment of the present invention.

[0063] The present invention provides a mobile robot positioning method, wherein the mobile robot is provided with a laser radar, and the method comprises:

[0064] Step 101, obtaining first radar point cloud information of the laser radar in the world coordinate system;

[0065] LiDAR (Light Detection and Ranging) is an active remote sensing technology that detects the position, speed, and characteristics of a target by emitting laser beams and receiving their reflected signals.

[0066] In an embodiment of the present invention, point cloud data may be collected by a laser radar, and the collected point cloud data may be converted into a world coordinate system to obtain first radar point cloud information.

[0067] Step 102: establishing a plurality of fitting planes based on the first radar point cloud information, and obtaining a normal of each fitting plane;

[0068] Fitting a plane is a basic operation in 3D environment modeling. Its core is to extract plane features from discrete point clouds through mathematical methods.

[0069] The normal to the fitted plane is the line perpendicular to the fitted plane.

[0070] After the first radar point cloud information is collected, several observed fitting planes may be established based on the first radar point cloud information, and a normal of each fitting plane may be obtained.

[0071] Step 103, calculating the minimum angle axis between the mobile robot coordinate system and each normal line;

[0072] The mobile robot coordinate system consists of the X, Y, and Z axes. The axis with the smallest angle to each normal is the axis with the smallest angle to the normal. If a fitted plane has the smallest angle with the X axis of the mobile robot coordinate system, then that plane's contribution to the X axis during robot movement is the greatest.

[0073] Step 104, calculating the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes;

[0074] Step 105, determining the underconstrained direction of the mobile robot according to the effective observation ratio;

[0075] Since each fitting plane contributes the most to one axis of the mobile robot's coordinate system, by calculating the minimum angle axis for each observed fitting plane and calculating the proportion of the effective observation plane for each axis, it is possible to determine in which axis the mobile robot is in an underconstrained state.

[0076] Step 106 : Adjust the noise in the underconstrained direction, and perform positioning of the mobile robot based on the adjusted noise of each axis of the mobile robot coordinate system.

[0077] For underconstrained directions, the mobile robot may experience motion degradation at any time. To prevent this phenomenon, it is necessary to increase the effective observation weight of the mobile robot in this direction.

[0078] In this embodiment of the present invention, the effective observation weight can be adjusted by adjusting the noise in the underconstrained direction. In practical scenarios, the noise and the effective observation weight are inversely proportional: the lower the noise, the greater the effective observation weight. By adjusting the effective observation weight in the underconstrained direction, the positioning accuracy of the mobile robot can be improved.

[0079] The present invention calculates the various fitting planes observed by the lidar, then calculates the minimum angle axis between the mobile robot coordinate system and each fitting plane, and judges the under-constrained direction of the mobile robot according to the effective observation ratio of each minimum angle axis, thereby achieving under-constrained judgment of the mobile robot observation and improving the positioning accuracy of the mobile robot by adaptively adjusting the threshold of the under-constrained direction.

[0080] See also Figure 2 , Figure 2 This is a flowchart of a method for positioning a mobile robot according to another embodiment of the present invention. Specifically, the method may include the following steps:

[0081] Step 201, obtaining the real-time position and posture of the mobile robot in the world coordinate system;

[0082] Step 202: obtaining second radar point cloud information collected by the laser radar in the mobile robot coordinate system;

[0083] Step 203: Convert the second radar point cloud information into a world coordinate system according to the real-time pose to obtain the first radar point cloud information;

[0084] In this embodiment of the present invention, a laser radar is installed in the mobile robot. Therefore, the position of the laser radar can be predicted using sensors such as an IMU, allowing the mobile robot's position and posture in the world coordinate system to be acquired in real time. Next, the second radar point cloud information collected by the laser radar in the mobile robot coordinate system is acquired. Then, using the position and posture transformation relationship between the laser radar and the IMU, the second radar point cloud information is converted from the mobile robot coordinate system to the world coordinate system to obtain the first radar point cloud information.

[0085] For example, for the second radar point P, its pose in the world coordinate system is T_w, then the radar point after converting the radar point P to the world coordinate system is P_w=T_w*P.

[0086] Step 204: establishing a plurality of fitting planes based on the first radar point cloud information, and obtaining a normal of each fitting plane;

[0087] After the first radar point cloud information is collected, several observed fitting planes may be established based on the first radar point cloud information, and a normal of each fitting plane may be obtained.

[0088] In one example, the first radar point cloud information includes a plurality of radar points, and step 204 may include the following sub-steps:

[0089] S41, sequentially searching for multiple adjacent points closest to each radar point;

[0090] S42, calculating the average coordinates of multiple adjacent points corresponding to each radar point to obtain the centroid coordinates;

[0091] S43, subtracting the centroid coordinate from the coordinate of each adjacent point to obtain a covariance matrix;

[0092] S44, calculating three eigenvalues ​​and eigenvectors corresponding to each eigenvalue according to the covariance matrix;

[0093] S45, determining whether the minimum eigenvalue is less than 0.001;

[0094] S46, if yes, take the eigenvector corresponding to the minimum eigenvalue as the normal vector, use the centroid coordinates and the normal vector to generate a fitting plane, and determine the normal vector as the normal line of the fitting plane.

[0095] In the implementation, for each radar point, the five closest points in the world coordinate system are searched for. The minimum eigenvalue of these five points is then calculated. If the minimum eigenvalue is less than 0.001, a fitting plane can be formed from these five points. The Euclidean distance from the radar point to this fitting plane is then calculated to construct a point-to-plane constraint.

[0096] In an example, the fitting process of nearby points is as follows:

[0097] 1. Calculate the centroid: Find the average value of all point coordinates to get the centroid coordinates (centroid_x, centroid_y, centroid_z).

[0098] 2. Calculate the covariance matrix: Subtract the centroid coordinates from the coordinates of each point to calculate the covariance matrix of the data matrix to reflect the distribution characteristics of the point cloud.

[0099] 3. Eigenvalue decomposition: For the covariance matrix, calculate its three eigenvalues ​​and their corresponding eigenvectors.

[0100] 4. Judge the plane fitting: Check whether the minimum eigenvalue is less than 0.001. If the condition is met, the fitting is considered successful, otherwise the fitting fails.

[0101] 5. Plane equation extraction: The eigenvector corresponding to the minimum eigenvalue is the plane normal vector (n_x, n_y, n_z). Then, using the centroid coordinates and the normal vector, construct the plane equation n_x(x-centroid_x)+n_y(y-centroid_y)+n_z(z-centroid_z)=0.

[0102] 6. Calculation of the distance from a point to a plane: Point P (x0, y0, z0). Calculation of the distance from a point to a plane: Use the distance formula from a point to a plane:

[0103] in, is the absolute value of the signed distance from point P to the plane.

[0104] Alternatively, after obtaining the fitting plane, calculate the eigenvector corresponding to the minimum eigenvalue of the fitting plane, which is the normal of the fitting plane.

[0105] Step 205, calculating the minimum angle axis between the mobile robot coordinate system and each normal line;

[0106] The mobile robot coordinate system consists of the X, Y, and Z axes. The axis with the smallest angle to each normal is the axis with the smallest angle to the normal. If a fitted plane has the smallest angle with the X axis of the mobile robot coordinate system, then that plane's contribution to the X axis during robot movement is the greatest.

[0107] In one example, step 205 may include the following sub-steps:

[0108] S51, calculating the angles between each axis of the mobile robot coordinate system and the normals of each fitting plane;

[0109] S52: Determine the minimum angle axis of each fitting plane according to the included angle.

[0110] In practice, for each fitted plane constructed using lidar observations, the angle between each axis of the moving robot coordinate system and the normal is calculated. This angle is controlled within a range of 0-90 degrees, and the axis of the moving robot coordinate system with the smallest angle to the normal of the fitted plane is selected. This axis is the minimum angle axis.

[0111] In one example, the feature vector is calculated as follows:

[0112] 1. Calculate the mean of each adjacent point;

[0113] 2. Construct the covariance matrix C based on the mean 3x3 ;

[0114] 3. Solve the characteristic equation |C 3x3 -λI|=0; I is the 3x3 identity matrix;

[0115] 4. Get the eigenvalues ​​λ1, λ2, and λ3. The vector corresponding to the eigenvalue is the eigenvector.

[0116] like Figure 3 As shown, Figure 3 is a side view of the mobile robot, where the normal to the fitted plane is n. The radar coordinate system and the mobile robot coordinate system are rigidly connected and coincident, representing the X, Y, and Z axes. For the observation in the upper right corner, the angle between the X axis and the normal to the fitted plane is the smallest. This indicates that the observed fitted plane has the greatest contribution to the X-axis during the mobile robot's movement, and the X-axis is the axis with the smallest angle. For the observation in the lower right corner, the angle between the Z axis and the normal to the fitted plane is the smallest. This indicates that the observed fitted plane has the greatest contribution to the Z-axis during the mobile robot's movement, and the Z-axis is the axis with the smallest angle.

[0117] like Figure 4 As shown, Figure 4 This is a top view of the mobile robot. For the observation information from above, the angle between the X-axis in the mobile robot coordinate system and the normal line n of the fitted plane is the smallest. Therefore, it can be assumed that the observed fitted plane has the greatest contribution to the X-axis during the mobile robot's movement, and the X-axis is the axis with the smallest angle. For the observation information from the right, the angle between the Y-axis in the mobile robot coordinate system and the normal line n of the fitted plane is the smallest. Therefore, it can be assumed that the observed fitted plane has the greatest contribution to the Y-axis during the mobile robot's movement, and the Y-axis is the axis with the smallest angle.

[0118] Step 206, calculating the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes;

[0119] In this embodiment of the present invention, the minimum angle axis corresponding to each observed fitting plane is calculated, and the proportion of the fitting plane corresponding to each of the X, Y, and Z axes in the total number of observed fitting planes is calculated. For example, in a radar image frame, there are 8,000 original point clouds, of which 6,000 point clouds constitute valid observations. Of the fitting planes formed by these valid processes, 3,000 correspond to the X axis, accounting for 0.5%, 800 correspond to the Y axis, accounting for 0.133%, and 2,200 correspond to the Z axis, accounting for 0.366%.

[0120] Step 207, determining the underconstrained direction of the mobile robot according to the effective observation ratio;

[0121] In this embodiment of the present invention, step 207 may include the following sub-steps:

[0122] S71, determining whether the proportion of each valid observation is less than a preset proportion threshold;

[0123] S72: If yes, determine that the direction of the minimum angle axis corresponding to the effective observation ratio is an under-constrained direction.

[0124] In the specific implementation, the effective observation ratio of each axis is analyzed. If the mobile robot is in an ideal environment, the effective observation ratio of each axis should be approximately 0.33. If the effective observation ratio of an axis is less than 0.15, the mobile robot is considered to be underconstrained in the direction of that axis. For example, if the effective observation ratio corresponding to the Y axis is 0.133, which is less than 0.15, the mobile robot is considered to be underconstrained on the Y axis.

[0125] Step 208 : Adjust the noise in the underconstrained direction, and perform positioning of the mobile robot based on the adjusted noise of each axis of the mobile robot coordinate system.

[0126] In this embodiment of the present invention, the effective observation weight can be adjusted by adjusting the noise in the underconstrained direction. In practical scenarios, the noise and the effective observation weight are inversely proportional: the lower the noise, the greater the effective observation weight. By adjusting the effective observation weight in the underconstrained direction, the positioning accuracy of the mobile robot can be improved.

[0127] In one example, step 208 may include the following sub-steps:

[0128] S81, obtain the ideal effective observation ratio;

[0129] S82, calculating the ratio of the effective observation ratio to the ideal effective observation ratio;

[0130] S83, obtaining the effective observation noise in the underconstrained direction;

[0131] S84, calculating the optimized noise based on the effective observation noise and the ratio;

[0132] S85, construct the Hessian matrix based on the optimized noise;

[0133] S86, mobile robot positioning based on the Hessian matrix.

[0134] In the specific implementation, the ideal effective observation ratio is 0.3. By calculating the ratio of the effective observation ratio of each axis to the ideal effective observation ratio, the noise adjustment ratio can be obtained. The calculation formula is as follows:

[0135] ratio=a / 0.33

[0136] Where a is the proportion of valid observations in the underconstrained direction.

[0137] After calculating the ratio of the adjusted noise, the noise in the underconstrained direction can be multiplied by the ratio to obtain the optimized noise, thereby increasing the effective observation weight in the underconstrained direction.

[0138] After optimizing the noise to achieve adaptive threshold adjustment, the Hessian matrix of each fitting plane can be calculated to realize the positioning of the mobile robot in an underconstrained environment, and the positioning posture observed by the lidar at this time is used to realize the positioning of the mobile robot.

[0139] The calculation formula of the Hessian matrix is ​​as follows:

[0140] 1. Define the observation model: ,in, is a nonlinear function, z is the observation value, and v is the observation noise.

[0141] 2. Define the residual function:

[0142] 3. Calculate the Jacobian matrix J:

[0143] 4. Construct the Hessian matrix H:

[0144] The present invention calculates the various fitting planes observed by the lidar, then calculates the minimum angle axis between the mobile robot coordinate system and each fitting plane, and judges the under-constrained direction of the mobile robot according to the effective observation ratio of each minimum angle axis, thereby achieving under-constrained judgment of the mobile robot observation and improving the positioning accuracy of the mobile robot by adaptively adjusting the threshold of the under-constrained direction.

[0145] See also Figure 5 , Figure 5 This is a structural block diagram of a mobile robot positioning device provided by an embodiment of the present invention.

[0146] An embodiment of the present invention provides a mobile robot positioning device, wherein the mobile robot is provided with a laser radar; the device includes:

[0147] A first radar point cloud information acquisition module 501 is used to acquire first radar point cloud information of the laser radar in the world coordinate system;

[0148] A fitting module 502 is configured to establish a plurality of fitting planes based on the first radar point cloud information and obtain a normal of each fitting plane;

[0149] The minimum angle axis calculation module 503 is used to calculate the minimum angle axis between the mobile robot coordinate system and each normal line;

[0150] The effective observation ratio calculation module 504 is used to calculate the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes;

[0151] An underconstrained direction determination module 505 is used to determine the underconstrained direction of the mobile robot according to the effective observation ratio;

[0152] The positioning module 506 is used to adjust the noise in the underconstrained direction and perform positioning of the mobile robot according to the adjusted noise of each axis of the mobile robot coordinate system.

[0153] In this embodiment of the present invention, the first radar point cloud information acquisition module 501 includes:

[0154] The real-time pose acquisition submodule is used to obtain the real-time pose of the mobile robot in the world coordinate system;

[0155] The second radar point cloud information acquisition submodule is used to obtain the second radar point cloud information collected by the laser radar in the mobile robot coordinate system;

[0156] The first radar point cloud information acquisition submodule is used to convert the second radar point cloud information into the world coordinate system according to the real-time posture to obtain the first radar point cloud information.

[0157] In this embodiment of the present invention, the minimum angle axis calculation module 503 includes:

[0158] An angle calculation submodule, used to calculate the angles between each axis of the mobile robot coordinate system and the normals of each fitting plane;

[0159] The minimum angle axis determination submodule is used to determine the minimum angle axis of each fitting plane according to the included angle.

[0160] In this embodiment of the present invention, the first radar point cloud information includes a plurality of radar points; the fitting module 502 includes:

[0161] The adjacent point search submodule is used to sequentially search for multiple adjacent points closest to each radar point;

[0162] The centroid coordinate calculation submodule is used to calculate the average coordinates of multiple adjacent points corresponding to each radar point to obtain the centroid coordinates;

[0163] The covariance matrix generation submodule is used to subtract the centroid coordinates from the coordinates of each adjacent point to obtain the covariance matrix;

[0164] The eigenvalue and eigenvector generation submodule is used to calculate the three eigenvalues ​​and the eigenvectors corresponding to each eigenvalue according to the covariance matrix;

[0165] The judgment submodule is used to determine whether the minimum eigenvalue is less than 0.001;

[0166] The fitting submodule is used to: if so, use the eigenvector corresponding to the minimum eigenvalue as the normal vector, use the centroid coordinates and the normal vector to generate a fitting plane, and determine the normal vector as the normal line of the fitting plane.

[0167] In this embodiment of the present invention, the underconstrained direction determination module 505 includes:

[0168] The effective observation ratio judgment submodule is used to judge whether the effective observation ratio is less than the preset ratio threshold;

[0169] The under-constrained direction judgment submodule is used to determine that if so, the direction of the minimum angle axis corresponding to the effective observation ratio is determined to be the under-constrained direction.

[0170] In this embodiment of the present invention, the positioning module 506 includes:

[0171] The ideal effective observation ratio acquisition submodule is used to obtain the ideal effective observation ratio;

[0172] The ratio calculation submodule is used to calculate the ratio of the effective observation ratio to the ideal effective observation ratio;

[0173] The effective observation noise acquisition submodule is used to obtain the effective observation noise in the underconstrained direction;

[0174] Noise optimization submodule, used to calculate the optimized noise based on the effective observation noise and the ratio;

[0175] Hessian matrix construction submodule, used to construct the Hessian matrix based on the optimized noise;

[0176] The positioning submodule is used to locate the mobile robot based on the Hessian matrix.

[0177] An embodiment of the present invention further provides an electronic device, the device including a processor and a memory:

[0178] The memory is used to store program codes and transmit the program codes to the processor;

[0179] The processor is configured to execute the mobile robot positioning method according to the instructions in the program code.

[0180] An embodiment of the present invention further provides a computer-readable storage medium, which is used to store program code, and the program code is used to execute the mobile robot positioning method of the embodiment of the present invention.

[0181] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0182] Each of the embodiments in the present specification is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts among the embodiments can be understood with reference to each other.

[0183] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, device, or computer program product. Therefore, the embodiments of the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. In addition, the embodiments of the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage, etc.) containing computer-usable program code.

[0184] The embodiments of the present application are described with reference to flowcharts and / or block diagrams according to the method, terminal device (system), and computer program product of the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing terminal device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing terminal device produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 an apparatus that performs the functions specified in one or more flows and / or blocks.

[0185] These computer program instructions can also be stored in a computer-readable memory that can direct the computer or other programmable data processing terminal device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a product including instruction apparatus that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 an apparatus that performs the functions specified in one or more flows and / or blocks.

[0186] These computer program instructions can also be loaded into a computer or other programmable data processing terminal device, so that a series of operation steps are performed on the computer or other programmable terminal device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable terminal device provide a process for implementing the functions specified in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 an apparatus that performs the functions specified in one or more flows and / or blocks.

[0187] Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they become aware of the basic creative concepts. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the embodiments of the present invention.

[0188] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with the relevant laws, regulations and standards of relevant countries and regions, and provide corresponding operation entrances for users to choose to authorize or refuse.

[0189] Finally, it should be noted that, in this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or terminal device that includes a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article, or terminal device. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article, or terminal device that includes the element.

[0190] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that the technical solutions described in the above embodiments can still be modified, or some of the technical features thereof can be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A mobile robot positioning method, characterized in that: The mobile robot is provided with a laser radar; the method comprises: Get the first radar point cloud information of the laser radar in the world coordinate system; Establishing a plurality of fitting planes according to the first radar point cloud information, and obtaining a normal of each of the fitting planes; Calculating the minimum angle axis between the mobile robot coordinate system and each of the normal lines; Calculate the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes; Determining an underconstrained direction of the mobile robot according to the effective observation ratio; The noise in the underconstrained direction is adjusted, and the mobile robot is positioned according to the adjusted noise of each axis of the mobile robot coordinate system.

2. The method according to claim 1, characterized in that The step of obtaining first radar point cloud information of the laser radar in the world coordinate system includes: Obtain the real-time position and posture of the mobile robot in the world coordinate system; Obtain the second radar point cloud information collected by the laser radar in the mobile robot coordinate system; The second radar point cloud information is converted into a world coordinate system according to the real-time pose to obtain the first radar point cloud information.

3. The method according to claim 1, characterized in that The step of calculating the minimum angle axis between the mobile robot coordinate system and each normal line comprises: Calculate the angles between each axis of the mobile robot coordinate system and the normals of each fitting plane; The minimum angle axis of each fitting plane is determined according to the included angle.

4. The method according to claim 1, wherein The first radar point cloud information includes a plurality of radar points; the step of establishing a plurality of fitting planes based on the first radar point cloud information and obtaining a normal of each of the fitting planes includes: Search for multiple nearby points closest to each radar point in sequence; Calculate the average coordinates of the plurality of adjacent points corresponding to each radar point to obtain the centroid coordinates; Subtract the centroid coordinates from the coordinates of each adjacent point to obtain a covariance matrix; Calculate three eigenvalues ​​and the eigenvectors corresponding to each eigenvalue according to the covariance matrix; Determine whether the minimum eigenvalue is less than 0.001; If so, the eigenvector corresponding to the minimum eigenvalue is used as a normal vector, the centroid coordinates and the normal vector are used to generate a fitting plane, and the normal vector is determined as a normal line of the fitting plane.

5. The method according to claim 1, wherein The step of determining the underconstrained direction of the mobile robot according to the effective observation ratio includes: Determine whether the proportion of each of the valid observations is less than a preset proportion threshold; If so, the direction of the minimum angle axis corresponding to the effective observation ratio is determined to be the under-constrained direction.

6. The method according to claim 1, characterized in that The step of adjusting the noise in the underconstrained direction and positioning the mobile robot according to the adjusted noise of each axis of the mobile robot coordinate system includes: Get the ideal effective observation ratio; Calculating the ratio of the effective observation ratio to the ideal effective observation ratio; Obtaining effective observation noise in the underconstrained direction; Calculating optimized noise according to the effective observation noise and the ratio; constructing a Hessian matrix based on the optimized noise; The mobile robot is positioned according to the Hessian matrix.

7. A mobile robot positioning device, characterized in that: The mobile robot is provided with a laser radar; the device comprises: A first radar point cloud information acquisition module is used to obtain the first radar point cloud information of the laser radar in the world coordinate system; a fitting module, configured to establish a plurality of fitting planes according to the first radar point cloud information, and obtain a normal of each of the fitting planes; A minimum angle axis calculation module, used to calculate the minimum angle axis between the mobile robot coordinate system and each normal line; The effective observation ratio calculation module is used to calculate the effective observation ratio of the fitting plane corresponding to each minimum angle axis in all fitting planes; an underconstrained direction determination module, configured to determine the underconstrained direction of the mobile robot according to the effective observation ratio; The positioning module is used to adjust the noise in the underconstrained direction and perform positioning of the mobile robot according to the adjusted noise of each axis of the mobile robot coordinate system.

8. The device according to claim 7, characterized in that The first radar point cloud information acquisition module includes: The real-time pose acquisition submodule is used to obtain the real-time pose of the mobile robot in the world coordinate system; The second radar point cloud information acquisition submodule is used to obtain the second radar point cloud information collected by the laser radar in the mobile robot coordinate system; The first radar point cloud information acquisition submodule is used to convert the second radar point cloud information into a world coordinate system according to the real-time posture to obtain the first radar point cloud information.

9. An electronic device, characterized in that: The device includes a processor and a memory: The memory is used to store program code and transmit the program code to the processor; The processor is used to execute the mobile robot positioning method according to any one of claims 1 to 6 according to the instructions in the program code.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium is used to store program code, and the program code is used to execute the mobile robot positioning method according to any one of claims 1 to 6.

Images