Limited space positioning closed loop and positioning data validity verification method and system based on global registration

Abstract

The invention belongs to the technical field of point cloud data processing, and more specifically relates to a method and a system for limited space positioning closed loop and positioning data validity verification based on global registration. The method comprises the steps of collecting radar point cloud data in real time through a radar and performing format conversion; according to the positioning data, the radar point cloud data and the three-dimensional point cloud map data output by the SLAM algorithm, the positioning data, the radar point cloud data and the three-dimensional point cloud map data are converted to the same coordinate system, noise reduction and voxel filtering processing are carried out, then point cloud registration operation is carried out, and a positioning error is obtained; carrying out matrix multiplication on the corrected real-time positioning data and the output real-time positioning data to obtain the corrected real-time positioning data; and carrying out validity verification on the corrected real-time positioning data. The problems that the positioning data output by the open source SLAM algorithm is directly sent to the radar, the control process is completely open-loop, the effectiveness verification is not performed on the positioning data output by the algorithm, the closed-loop control cannot be realized, and the stability of the control system is poor are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of point cloud data processing, and more specifically, relates to a method and system for verifying the validity of limited space positioning closed loop and positioning data based on global registration. Background Art

[0002] Currently, in many outdoor scenes, such as power transmission channels, photovoltaic panels, and water conservancy environments, radars (such as drones, etc.) have the functions of autonomous inspection and line-flying, which reduces the pressure of manual inspection in remote areas. However, for the inspection of the above outdoor scenes, radars rely on GPS positioning technology, or rely on strong GPS signals. In more harsh scenes such as underground pipelines, underground mines, and closed but dangerous indoor limited space scenes, radars cannot locate due to weak GPS signals. However, these scenes need radars to explore, which can not only reduce labor costs, but more importantly, ensure personal safety.

[0003] At present, the implementation technology for limited space positioning is relatively mature. For example, SLAM (Simultaneous Localization and Mapping) technology only needs one camera or one laser radar to achieve limited space positioning. Chinese patent document CN 109671119 A discloses a SLAM-based indoor positioning method and device. The SLAM algorithm in the patent quickly constructs an image database with posture information, and completes indoor positioning based on the constructed image database.

[0004] Currently, the use of this technology is rather crude. Usually, the positioning data output by the open source SLAM algorithm is sent directly to the radar, and the radar moves. The control process is completely open-loop, and the validity of the positioning data output by the SLAM algorithm is not verified. The control closed loop cannot be achieved, the control system stability is poor, and the radar operation reliability is not high. Summary of the invention

[0005] The present invention aims to overcome at least one defect of the above-mentioned prior art and provides a method for limited space positioning closed loop and positioning data validity verification based on global registration, pre-running a SLAM algorithm to generate a reliable three-dimensional map of the indoor scene, and then aligning the radar data obtained in real time by the radar with the above map to obtain the positioning data output by the current SLAM algorithm after positioning error correction, realize control closed loop, and verify the corrected positioning data, thereby further improving the stability of the control system and making the radar operation more reliable.

[0006] The invention also discloses a system for limited space positioning closed loop and positioning data validity verification based on global registration.

[0007] The detailed technical scheme of the present invention is as follows:

[0008] A method for closed-loop positioning in a limited space and verification of positioning data validity based on global registration, the method comprising:

[0009] S1, collect radar point cloud data in real time through radar;

[0010] S2, converting the format of the collected radar point cloud data into the standard input format required by the open source SLAM algorithm, and inputting the converted radar point cloud data into the open source SLAM algorithm;

[0011] S3: Run the open source SLAM algorithm to output positioning data in real time, then use the handheld radar to circle the indoor scene to be positioned, record the starting point position, and obtain the three-dimensional point cloud map data of the indoor inspection scene;

[0012] S4, repeat S1-S3, and when S3 is repeated to run the open source SLAM algorithm, the starting point position is consistent with the starting point position first described in S3;

[0013] S5. Output positioning data, radar point cloud data and three-dimensional point cloud map data according to the SLAM algorithm, convert the radar point cloud data and the three-dimensional point cloud map data to the same coordinate system through three-dimensional rigid body transformation technology, and perform noise reduction and voxel filtering. Perform point cloud registration operation on the processed data, and the obtained transformation matrix is ​​the positioning error;

[0014] S6, performing matrix multiplication on the obtained positioning error and the real-time positioning data output by the SLAM algorithm to obtain corrected real-time positioning data;

[0015] S7. Perform validity check on the corrected real-time positioning data.

[0016] Furthermore, the radar point cloud data and the three-dimensional point cloud map data are converted to the same coordinate system by using the three-dimensional rigid body transformation technology, specifically including:

[0017] S51, the starting point position of the radar collected by the 3D point cloud map generation module is P0, and a coordinate system is established with P0 as the origin. The obtained 3D point cloud map data is Real-time collection of radar point cloud data Where n>

[0018] m,x i ,y i ,z i is the 3D point coordinate data of the i-th point in the point cloud map, n is the total number of 3D points in the point cloud map, x j ,y j ,z jis the 3D point coordinate data of the jth point in the real-time collected radar point cloud data, and m is the total number of 3D points contained in the real-time collected radar point cloud data;

[0019] S52, from the current time t o The open source SLAM algorithm starts running at the initial position P0, t k The real-time positioning data output at all times is matrix J, which is a four-dimensional transformation matrix, which represents the three-axis angle change and position change of the current radar relative to P0, and can be converted into a rotation matrix R and a translation matrix Q;

[0020] S53, converting the three-dimensional point cloud map data recorded with P0 as the reference coordinate system into k The map data of the reference coordinate system at the time position is calculated as shown in (1), where To obtain t k Map data with the time position as the reference coordinate system, J -1 represents the inverse matrix of the positioning data matrix J, and T is the transpose operation of the matrix.

[0021]

[0022] The point cloud data collected by the radar is already in t k The time position is the data of the reference coordinate system.

[0023] Furthermore, the denoising and voxel filtering process specifically comprises the following operations:

[0024] S501. The effective scanning range of the radar is set to E meters. The Euclidean distance between the three-dimensional point cloud map point and the origin is calculated by traversing and calculating the formula as shown in (2):

[0025]

[0026] Only map points with dist less than E are retained, which is the noise reduction process;

[0027] S502, then, voxel filtering is performed on the three-dimensional point cloud map data and the real-time collected radar point cloud data respectively, and the process is as follows:

[0028] S5021, set the voxel size to w meters;

[0029] S5022. Create a voxel grid: Create a three-dimensional voxel grid of size x_voxels×y_voxels×z_voxels according to the determined voxel size. The calculation formulas for x_voxels, y_voxels, and z_voxels are as follows. Assuming that the ranges of the point cloud data in the x, y, and z dimensions are [x_min, x_max], [y_min, y_max], and [z_min, z_max] respectively, the number of voxels required in each dimension can be calculated:

[0030] x_voxels=(x_max-x_min) / w (3);

[0031] y_voxels=(y_max-y_min) / w (4);

[0032] z_voxels=(z_max-z_min) / w (5);

[0033] Finally, the size of the created three-dimensional voxel grid is (x_voxels, y_voxels, z_voxels);

[0034] S5023, filling voxel grid: assigning each point in the point cloud to a corresponding voxel grid;

[0035] Traverse each point in the point cloud, and for each point cloud point (x, y, z), calculate the index in the voxel grid according to its coordinate position:

[0036] x_index = (x-x_min) / w (6);

[0037] y_index = (y - y_min) / w (7);

[0038] z_index = (z-z_min) / w (8);

[0039] In formulas (3) to (8), x_min, y_min, and z_min are the minimum values ​​of the point cloud data in each dimension;

[0040] S5024, perform filtering operation: take the average of the point cloud points with the same x_index, y_index, and z_index and store it in the index of the voxel grid;

[0041] Output filtering results: traverse all voxel grids and merge the point cloud points in non-empty voxel grids as filtered point cloud data.

[0042] Specifically, radar data is part of the 3D point cloud map data, and 3D point cloud data is the time integral of radar data or the sum accumulated over a certain period of time. The radar is moving. Point cloud registration is to find the position of the radar data in the map data, or the position of the radar in the map where the radar data was scanned to obtain the radar data, given a set of radar data and a set of map data.

[0043] Furthermore, the specific operation of the point cloud registration is:

[0044] The three-dimensional point cloud map data obtained after filtering is The target point cloud is recorded as the target point cloud, and the real-time collected radar point cloud data after filtering is Denoted as the source point cloud, where X i ,Y i ,Z i is the 3D point coordinate data of the i-th point in the target point cloud, X j ,Y j ,Z j is the 3D point coordinate data of the jth point in the target point cloud;

[0045] Find the nearest point set: traverse the target point cloud data, set the distance threshold, find the nearest point that corresponds to the source point cloud one by one, and record the set of points found in the target point cloud as The corresponding set of points in the source point cloud is Where P i represents the coordinates of the i-th 3D point in the target point cloud, S j Represents the 3D point coordinates of the jth point in the source point cloud;

[0046] Centralized point cloud: Calculate the centroid of the nearest point set of the target point cloud and the corresponding point set of the source point cloud respectively, and centralize each point to obtain the target point cloud point set P relative to the centroid. i ′, source point cloud set S i ′, the calculation formula is as follows:

[0047]

[0048] S j ′=S j -c s (11);

[0049] P i ′=P i -c p (12);

[0050] Calculate the transformation matrix T of the source point cloud point set relative to the target point cloud point set, that is, the positioning error. The calculation formula is as follows:

[0051]

[0052] R=VU T (14);

[0053] t=c p -Rc s (15);

[0054]

[0055] Among them, H represents the covariance matrix of the source point cloud and the target point cloud. H is decomposed into singular values ​​to obtain U, V, σ. The rotation transformation R and translation transformation t of the source point cloud point set relative to the target point cloud point set are calculated according to the decomposed matrix, and finally the transformation matrix T is obtained; among them, U is an orthogonal matrix composed of the left singular vectors of the matrix H; σ is a diagonal matrix, the diagonal elements are composed of the singular values ​​of the matrix H, the diagonal is the singular value, and the rest is 0; V T is the transpose of V, which is an orthogonal matrix consisting of the right singular vectors of the matrix H.

[0056] Furthermore, according to the three-axis speed and three-axis angular speed of the radar in real time, the last output positioning data, and the time interval from the last output of the positioning data, i.e., the time step, the estimated value of the current positioning data is calculated. If the difference between the corrected positioning data actually output by S6 and the estimated value is greater than the set threshold, that is, the verification is invalid, the estimated value is used as the current positioning data. If the difference is less than the set threshold, that is, the verification is valid, the positioning data output by S6 is used.

[0057] The validity check of the corrected real-time positioning data specifically includes:

[0058] Let the last moment positioning data output by the positioning correction module be D t-1 , the positioning data output at the current moment is D t , the current positioning data estimated by the validity verification module is recorded as D′ t , the time step is 1s, assuming that the frequency of obtaining the radar three-axis angular velocity and three-axis velocity is γ Hz, the x, y, z three-axis angular velocity obtained in real time within the time step of 1s is recorded as: The speeds of the three axes are recorded as:

[0059] Then D is extracted by the following formulas: t-1 and D t The three-axis rotation angle and translation vector, assuming that the radar rotation order is ZYX, the calculation formula is as follows:

[0060]

[0061] θ Y =arctan(R 32 / R 33 ) (18);

[0062]

[0063] θ z =arctan(R 21 / R 11 )(20);

[0064] The three-axis angles and translation vectors at time t-1 and time t are respectively recorded as L t-1 , L t ;

[0065] According to the following formula, the positioning data output at the current moment is D t Is it effective:

[0066]

[0067] ‖L t -L t-1 -L′ t ‖≤L yuzhi (twenty three);

[0068] Among them, the || operator means to find the modulus of the vector, θ yuzhi is the artificially set allowable three-axis angle positioning error threshold, L yuzhi It is an artificially set allowable three-axis translation positioning error threshold.

[0069] In another aspect of the present invention, a system for closed-loop positioning in a limited space and validation of positioning data based on global registration is provided, comprising:

[0070] Radar point cloud data real-time acquisition module, point cloud data format conversion module, 3D point cloud map generation module, positioning error calculation module, real-time positioning correction module, positioning data validity verification module;

[0071] The radar point cloud data real-time acquisition module is used to use a microcomputer and an open source program provided by a radar manufacturer to allow the microcomputer to communicate with the radar to obtain data from radar modules of different forms;

[0072] The point cloud data format conversion module is used to convert the collected radar point cloud data into a standard input format of the three-dimensional point cloud map generation module;

[0073] The three-dimensional point cloud map generation module is used to run an open source SLAM algorithm on a microcomputer, and to make the radar circle around the scene to be positioned by hand-held or other methods to obtain the three-dimensional point cloud map data of the scene;

[0074] The positioning error calculation module is used to convert the real-time radar data and the three-dimensional map data into the same coordinate system according to the positioning data, radar point cloud data and three-dimensional point cloud map data output by the SLAM algorithm using the three-dimensional rigid body transformation technology, and perform noise reduction and voxel filtering. The processed data is subjected to point cloud registration operation, and the obtained transformation matrix is ​​recorded as the positioning error;

[0075] The real-time positioning correction module is used to perform matrix multiplication on the positioning error obtained by the positioning error calculation module and the real-time positioning data output by the SLAM algorithm to obtain the corrected real-time positioning data;

[0076] The positioning data validity verification module is used to calculate the estimated value of the current positioning data based on the three-axis speed and three-axis angular velocity of the radar in real-time operation, the positioning data output last time, and the time interval from the output of the last positioning data. The difference between the real-time positioning data output by the real-time positioning correction module and the estimated value is calculated, and the difference is compared with the set threshold to determine the radar positioning data of this time.

[0077] On the other hand, the present invention also includes an electronic device, including a memory, a processor, and a program stored in the memory and executable on the processor. When the processor executes the program, the steps in the method for closed-loop positioning and positioning data validity verification based on global registration in a limited space as described in the first aspect of the present invention are implemented.

[0078] On the other hand, the present invention also provides a computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the steps in a method for closed-loop positioning of limited space and verification of positioning data validity based on global registration as described in the first aspect of the present invention.

[0079] Compared with the prior art, the present invention has the following beneficial effects:

[0080] The present invention provides a method and system for a limited space positioning closed loop and positioning data validity verification based on global registration. By applying relevant technologies such as global registration and validity verification to the limited space positioning of radar indoors, a control closed loop is realized, which greatly improves the stability of the radar's indoor positioning and makes the radar operation more reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 It is a schematic flow diagram of the method of the present invention.

[0082] Figure 2 It is a comparison diagram of the flight trajectories of the present invention and the existing method in the substation indoor inspection scenario in Example 1 of the present invention.

[0083] Figure 3 It is a schematic diagram of the reference trajectory of the human-machine indoor flight in Example 1 of the present invention.

[0084] Figure 4 This is an example picture taken in an indoor inspection scene in Example 1 of the present invention.

[0085] Figure 5 This is a system structure block diagram of Example 2 of the present invention. DETAILED DESCRIPTION

[0086] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0087] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0088] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0089] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0090] Example 1

[0091] This embodiment provides a method for closed-loop positioning in a limited space and validating positioning data based on global registration, such as Figure 1 As shown, the method includes:

[0092] S1, collect radar point cloud data in real time through radar;

[0093] This embodiment uses a certain brand of three-dimensional laser radar and a certain brand of microcomputer to obtain real-time radar data. Among them, the radar has a horizontal scanning field of view of 360 degrees, a vertical scanning field of view of about 60 degrees, a maximum scanning range of up to 100m, and an effective scanning range of usually 40m. To obtain the radar data, it is necessary to install the open source driver provided by the radar manufacturer on the computer and configure the radar and microcomputer IP addresses in the driver.

[0094] S2, converting the format of the collected radar point cloud data into the standard input format required by the open source SLAM algorithm, and inputting the converted radar point cloud data into the open source SLAM algorithm;

[0095] This embodiment selects the fast-lio algorithm as the open source SLAM algorithm. The radar point cloud data format obtained in S1 is just the standard input format required by the open source algorithm. Its point cloud point information includes timestamp, x, y, z three-axis translation vector, reflectivity and other data. In addition, the radar used in this embodiment inherits imu internally, and in addition to point cloud data, it also provides imu data of the radar.

[0096] S3. Run the open source SLAM algorithm to obtain data:

[0097] Run the open source SLAM algorithm to output positioning data in real time, then use the handheld radar to circle the indoor scene to be positioned, record the starting point position, and obtain the 3D point cloud map data of the indoor inspection scene;

[0098] The open source SLAM algorithm is run on the microcomputer that obtains the radar data. The handheld radar is circled in the indoor scene selected in this embodiment to obtain the three-dimensional point cloud map data of the indoor inspection scene. Figure 3 shown.

[0099] S4, repeat S1-S3, and when S3 is repeated to run the open source SLAM algorithm, the starting point position is consistent with the starting point position first described in S3. Preferably, this embodiment repeats S1-S3 once.

[0100] S5. Unify the coordinate system and perform data processing and point cloud registration operations:

[0101] According to the positioning data, radar point cloud data and three-dimensional point cloud map data output by the SLAM algorithm, the radar point cloud data and the three-dimensional point cloud map data are converted to the same coordinate system through the three-dimensional rigid body transformation technology, and noise reduction and voxel filtering are performed. The processed data is subjected to point cloud registration operation, and the obtained transformation matrix is ​​recorded as the positioning error.

[0102] The conversion to the same coordinate system may be a standard coordinate system, and the preferred embodiment of the present invention is: according to the positioning data output by the SLAM algorithm, the coordinates of the three-dimensional point cloud map data obtained by S3 are converted from the original starting point as the origin to the same coordinate system with the current radar position as the origin by using the three-dimensional rigid body transformation, that is, the radar point cloud data and the three-dimensional point cloud map data are converted to the same coordinate system with the current radar position as the origin by the three-dimensional rigid body transformation technology:

[0103] S51, the starting point position of the radar collected by the 3D point cloud map generation module is P0, and a coordinate system is established with P0 as the origin. The obtained 3D point cloud map data is Real-time collected radar point cloud data, where n>m, x i ,y i ,z i is the 3D point coordinate data of the i-th point in the point cloud map, n is the total number of 3D points in the point cloud map, x j ,y j ,z j is the 3D point coordinate data of the jth point in the real-time collected radar point cloud data, and m is the total number of 3D points contained in the real-time collected radar point cloud data;

[0104] S52, from the current time t o The open source SLAM algorithm starts running at the initial position P0, t k The real-time positioning data output at all times is matrix J, which is a four-dimensional transformation matrix, representing the three-axis angle change and position change of the current radar relative to P0, converted into a rotation matrix R and a translation matrix Q;

[0105] S53, converting the three-dimensional point cloud map data recorded with P0 as the reference coordinate system into k The map data of the reference coordinate system at the time position is calculated as follows:

[0106]

[0107] To obtain t k Map data J with time position as reference coordinate system -1 It represents the inverse matrix of the positioning data matrix J, and the superscript T is the transpose operation of the matrix.

[0108] Preferably, the denoising and voxel filtering processing specifically includes:

[0109] S501. The effective scanning range of the radar is set to E meters, and the preferred effective scanning range is 40 meters. The Euclidean distance between the three-dimensional point cloud map point and the origin is calculated by traversing and calculating the formula as follows:

[0110]

[0111] Only map points with dist less than 40 are retained, and the noise reduction process is completed;

[0112] S502: Then, voxel filtering is performed on the three-dimensional point cloud map data and the radar point cloud data collected in real time, respectively, specifically:

[0113] S5021, setting the voxel size to w meters. Preferably, in this embodiment, the voxel size in the source point cloud data voxel filtering is set to 0.2 meters, and the voxel size in the target point cloud voxel filtering is set to 0.5 meters;

[0114] S5022, create voxel grid: according to the determined voxel size, create a three-dimensional voxel grid of size x_voxels×y_voxels×z_voxels, and let the range of point cloud data in x, y, and z dimensions be [x_min, x_max], [y_min, y_max], and [z_min, z_max] respectively. Then the number of voxels required in each dimension can be calculated. The calculation formulas of x_voxels, y_voxels, and z_voxels are as follows:

[0115] x_voxels=(x_max-x_min) / w(3);

[0116] y_voxels=(y_max-y_min) / w(4);

[0117] z_voxels=(z_max-z_min) / w(5);

[0118] Finally, the size of the created three-dimensional voxel grid is (x_voxels, y_voxels, z_voxels);

[0119] S5023, fill voxel grid: assign each point in the point cloud to the corresponding voxel grid. The specific process is as follows: first traverse each point in the point cloud, and for each point, calculate the index in the voxel grid according to its coordinate position. Assume that the coordinates of a point cloud point are (x, y, z).

[0120] x_index = (x-x_min) / w (6)

[0121] y_index = (y-y_min) / w (7)

[0122] z_index = (z-z_min) / w (8)

[0123] Among them, x_min, y_min, z_min are the minimum values ​​of the point cloud data in each dimension;

[0124] S5024, perform filtering operation: take the average of the point cloud points with the same x_index, y_index, and z_index and store it in the index of the voxel grid.

[0125] Preferably, performing the point cloud registration operation specifically includes:

[0126] The three-dimensional point cloud map data obtained after filtering is The target point cloud is recorded as the target point cloud, and the real-time collected radar point cloud data after filtering is Denoted as the source point cloud, where X i,Y i ,Z i is the 3D point coordinate data of the i-th point in the target point cloud, X j ,Y j ,Z j is the 3D point coordinate data of the jth point in the target point cloud;

[0127] Find the nearest point set: traverse the target point cloud data, set the distance threshold, find the nearest point that corresponds to the source point cloud one by one, and record the set of points found in the target point cloud as The corresponding set of points in the source point cloud is Where P i represents the coordinates of the i-th 3D point in the target point cloud, S j Represents the 3D point coordinates of the jth point in the source point cloud;

[0128] Centralized point cloud: Calculate the centroid of the nearest point set of the target point cloud and the corresponding point set of the source point cloud respectively, and centralize each point to obtain the target point cloud point set P relative to the centroid. i ′, source point cloud set S i ′, the calculation formula is as follows:

[0129]

[0130] S j ′=S j -c s (11);

[0131] P i ′=P i -c p (12);

[0132] Calculate the transformation matrix T of the source point cloud point set relative to the target point cloud point set, that is, the positioning error. The calculation formula is as follows:

[0133]

[0134] R=VU T (14);

[0135] t=c p -Rc s (15);

[0136]

[0137] Among them, H represents the covariance matrix of the source point cloud and the target point cloud. H is decomposed into three matrices: U, V, and σ. The rotation transformation R and translation transformation t of the source point cloud point set relative to the target point cloud point set are calculated based on the decomposed matrices. Finally, the transformation matrix T is obtained, which is the positioning error.

[0138] The transformation matrix T finally obtained in this embodiment is:

[0139]

[0140] Specifically, U is an orthogonal matrix consisting of the left singular vectors of the matrix H; σ is a diagonal matrix whose diagonal elements consist of the singular values ​​of the matrix H, with singular values ​​on the diagonal and the rest being 0; V T is the transpose of V, which is an orthogonal matrix consisting of the right singular vectors of the matrix H.

[0141] S6. Real-time positioning correction:

[0142] Perform matrix multiplication on the obtained positioning error and the real-time positioning data output by the SLAM algorithm to obtain the corrected real-time positioning data;

[0143] Perform real-time positioning correction. The real-time positioning data output by the SLAM algorithm running in step S3 is:

[0144]

[0145] S7. Perform validity verification on the corrected real-time positioning data, specifically including:

[0146] Assume that the positioning data output by the positioning correction module at the last moment is D t-1 , the positioning data output at the current moment is D t , the current positioning data estimated by the validity verification module is recorded as D′ t , the time step is 1s, assuming that the frequency of obtaining the radar three-axis angular velocity and three-axis velocity is γ Hz, the x, y, z three-axis angular velocity obtained in real time within the time step of 1s is recorded as: The speeds of the three axes are recorded as:

[0147] Then D is extracted by the following formulas: t-1 and D t The three-axis rotation angle and translation vector, assuming that the radar rotation order is ZYX, the calculation formula is as follows:

[0148]

[0149] θ Y =arctan(R 32 / R 33 )(18)

[0150]

[0151] θ z=arctan(R 21 / R 11 )(20)

[0152] The three-axis angles and translation vectors at time t-1 and time t are respectively recorded as L t-1 , L t ;

[0153] According to the following formula, the positioning data output at the current moment is D t Whether it is valid or not means: if the difference between the corrected positioning data actually output by S6 and the estimated value is greater than the set threshold, the verification is invalid, and the estimated value is used as the current positioning data; if the difference is less than the set threshold, the verification is valid, and the positioning data output by S6 is used:

[0154]

[0155]

[0156] ‖L t -L t-1 -L′ t ‖≤L yuzhi (twenty three)

[0157] Among them, the || operator means to find the modulus of the vector, θ yuzhi is the artificially set allowable three-axis angle positioning error threshold, L yuzhi It is an artificially set allowable three-axis translation positioning error threshold.

[0158] In S7, the positioning data validity verification module calculates the estimated value of the current positioning data based on the three-axis speed and three-axis angular velocity of the radar in real time, the last output positioning data, and the time interval from the last output of the positioning data. The three-axis angle allowable threshold is set to 10 degrees, the displacement allowable deviation threshold is set to 0.5m, and the calculation is recorded. Among the 30 positioning data, the estimated value is used 4 times, and 26 times are the positioning data output by the real-time positioning correction module.

[0159] In this embodiment, taking the installation of a drone as an example, a system for limited space positioning closed-loop and positioning data validity verification based on global registration is installed on a drone of a certain brand together with other required software and hardware structures, and tested in a substation room.

[0160] First, determine the take-off position of the drone, and select three fixed points in the substation room as waypoints, which are recorded as waypoint 1, waypoint 2, and waypoint 3 respectively;

[0161] The fixed drone flight altitude is 2m, with the take-off position as the origin, the origin is connected to waypoint 1, waypoint 1 is connected to waypoint 2, and so on as the reference trajectory of the drone indoor flight, such as Figure 3 As shown in the figure, the light blue path is the reference trajectory of the drone's indoor flight, and the dark blue is the drone's current position.

[0162] Taking the take-off position as the origin and the radar coordinate system as the reference coordinate system, use a ruler to measure the distance in meters to obtain the actual coordinates of several points in the reference trajectory of the drone. Preferably, only twenty points are measured in this embodiment;

[0163] like Figure 2 The figure shows the comparison between the reference route, the actual flight route of the drone using the method disclosed in this embodiment, and the actual flight route of other disclosed methods. It can be seen from the figure that compared with other disclosed indoor positioning methods, the method disclosed in the present invention is the closest to the actual positioning trajectory;

[0164] Based on the accurate positioning method of the present invention, it is possible to accurately photograph the target in the scene and develop more derivative technologies such as image processing on this basis, such as Figure 4 As shown, the drone realizes accurate positioning and photographing of meter targets in the substation based on the disclosed method, and recognizes the meter readings based on this;

[0165] Figure 4 What is displayed is the pressure gauge in the substation room. 0.43 represents its degree, but this has nothing to do with the content protected by this patent. It is only based on the reading technology derived from this patent and the positioning technology based on this patent that can realize the inspection and photography of targets by drones indoors.

[0166] Example 2

[0167] This embodiment provides a system for implementing a method for closed-loop positioning in a limited space and validating positioning data based on global registration, such as Figure 5 As shown, it includes: a radar point cloud data real-time acquisition module, a point cloud data format conversion module, a three-dimensional point cloud map generation module, a positioning error calculation module, a real-time positioning correction module, and a positioning data validity verification module;

[0168] The radar point cloud data real-time acquisition module is used to use a microcomputer and an open source program provided by a radar manufacturer to allow the microcomputer to communicate with the radar to obtain data from radar modules of different forms;

[0169] The point cloud data format conversion module is used to convert the collected radar point cloud data into a standard input format of the three-dimensional point cloud map generation module;

[0170] The three-dimensional point cloud map generation module is used to run the SLAM algorithm on a microcomputer, and to make the radar circle around the scene to be positioned by hand-held or other methods to obtain the three-dimensional point cloud map data of the scene;

[0171] The positioning error calculation module is used to convert the real-time radar data and the three-dimensional map data into the same coordinate system according to the positioning data, radar point cloud data and three-dimensional point cloud map data output by the SLAM algorithm using the three-dimensional rigid body transformation technology, and perform noise reduction and voxel filtering. The processed data is subjected to point cloud registration operation, and the obtained transformation matrix is ​​recorded as the positioning error;

[0172] The real-time positioning correction module is used to perform matrix multiplication on the positioning error obtained by the positioning error calculation module and the real-time positioning data output by the SLAM algorithm to obtain the corrected real-time positioning data;

[0173] The positioning data validity verification module is used to calculate the estimated value of the current positioning data based on the three-axis speed and three-axis angular velocity of the radar in real-time operation, the positioning data output last time, and the time interval from the output of the last positioning data. The difference between the real-time positioning data output by the real-time positioning correction module and the estimated value is calculated, and the difference is compared with the set threshold to determine the radar positioning data of this time.

[0174] Example 3

[0175] This embodiment also provides a computer-readable storage medium storing executable instructions, which, when executed, enable the machine to execute the method of finite space positioning closed loop and positioning data validity verification based on global registration as described above.

[0176] Specifically, a system or device equipped with a readable storage medium can be provided, on which software program codes that implement the functions of any of the above-mentioned embodiments are stored, and a computer or processor of the system or device can read and execute instructions stored in the readable storage medium.

[0177] In this case, the program code itself read from the computer-readable medium can realize the function of any one of the above embodiments, and thus the computer-readable code and the computer-readable storage medium storing the computer-readable code constitute part of this specification.

[0178] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, the program code may be downloaded from a server computer or a cloud via a communication network.

[0179] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may 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-ROM, optical storage, etc.) containing computer-usable program code.

[0180] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0181] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0182] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0183] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the technical solution of the present invention, and are not intended to limit the specific implementation methods of the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the claims of the present invention shall be included in the protection scope of the claims of the present invention.

Claims

1. A method for closed-loop positioning in a limited space and verification of the validity of positioning data based on global registration, characterized in that: The method comprises: S1, collect radar point cloud data in real time through radar; S2, converting the format of the collected radar point cloud data into the standard input format required by the open source SLAM algorithm, and inputting the converted radar point cloud data into the open source SLAM algorithm; S3: Run the open source SLAM algorithm to output positioning data in real time, then use the handheld radar to circle the indoor scene to be positioned, record the starting point position, and obtain the three-dimensional point cloud map data of the indoor inspection scene; S4, repeat S1-S3, and when S3 is repeated to run the open source SLAM algorithm, the starting point position is consistent with the starting point position first described in S3; S5. According to the positioning data, radar point cloud data and three-dimensional point cloud map data output by the SLAM algorithm, the radar point cloud data and the three-dimensional point cloud map data are converted to the same coordinate system through the three-dimensional rigid body transformation technology, and noise reduction and voxel filtering are performed. The processed data is subjected to point cloud registration operation, and the obtained transformation matrix is ​​the positioning error; S6, performing matrix multiplication of the obtained positioning error and the real-time positioning data output by the SLAM algorithm to obtain corrected real-time positioning data; S7. Perform validity check on the corrected real-time positioning data.

2. The method for closed-loop positioning and positioning data validity verification based on global registration in limited space according to claim 1, characterized in that: The method of converting the radar point cloud data and the three-dimensional point cloud map data into the same coordinate system by using the three-dimensional rigid body transformation technology specifically includes: S51, the starting point position of the radar collected by the 3D point cloud map generation module is P0, and a coordinate system is established with P0 as the origin. The obtained 3D point cloud map data is Real-time collected radar point cloud data, where n>m, x i ,y i ,z i is the 3D point coordinate data of the i-th point in the point cloud map, n is the total number of 3D points in the point cloud map, x j ,y j ,z j is the 3D point coordinate data of the jth point in the real-time collected radar point cloud data, and m is the total number of 3D points contained in the real-time collected radar point cloud data; S52, from the current time t o The open source SLAM algorithm starts running at the initial position P0, t k The real-time positioning data output at all times is matrix J; matrix J is a four-dimensional transformation matrix, which represents the three-axis angle change and position change of the current radar relative to P0, and can be converted into a rotation matrix R and a translation matrix Q; S53, converting the three-dimensional point cloud map data recorded with P0 as the reference coordinate system into k The map data of the reference coordinate system at the time position is calculated as shown in (1), where To obtain t k Map data with the time position as the reference coordinate system, J -1 represents the inverse matrix of the positioning data matrix J, and the superscript T is the transpose operation of the matrix; 3. The method for closed-loop positioning and positioning data validity verification based on global registration in limited space according to claim 2, characterized in that: The denoising and voxel filtering process specifically involves the following operations: S501. The effective scanning range of the radar is set to E meters. The Euclidean distance between the three-dimensional point cloud map point and the origin is calculated by traversing and calculating the formula as follows: Only map points with dist less than E are retained, which is the noise reduction process; S502, then, voxel filtering is performed on the three-dimensional point cloud map data and the real-time collected radar point cloud data respectively, and the process is as follows: S5021, set the voxel size to w meters; S5022, create voxel grid: according to the determined voxel size, create a three-dimensional voxel grid of size x_voxels×y_voxels×z_voxels, let the range of point cloud data in x, y, z dimensions be [x_min,x_max], [y_min,y_max], [z_min,z_max] respectively, and then calculate the number of voxels required in each dimension, then the calculation of x_voxels, y_voxels, z_voxels is as follows: x_voxels=(x_max-x_min) / w (3); y_voxels=(y_max-y_min) / w (4); z_voxels=(z_max-z_min) / w (5); In formulas (3) to (5), x_min, y_min, and z_min are the minimum values ​​of the point cloud data in each dimension; Finally, the size of the created three-dimensional voxel grid is (x_voxels, y_voxels, z_voxels); S5023, filling voxel grid: assigning each point in the point cloud to a corresponding voxel grid; Traverse each point in the point cloud, and for each point cloud point (x, y, z), calculate the index in the voxel grid based on its coordinate position; x_index=(x-x_min) / w (6); y_index = (y - y_min) / w (7); z_index=(z-z_min) / w (8); S5024, perform filtering operation: take the average of the point cloud points with the same x_index, y_index, and z_index and store it in the index of the voxel grid; Output filtering results: traverse all voxel grids and merge the point cloud points in non-empty voxel grids as filtered point cloud data.

4. The method for closed-loop positioning and positioning data validity verification in a limited space based on global registration according to claim 3, characterized in that: The specific operation of the point cloud registration is: The three-dimensional point cloud map data obtained after filtering is The target point cloud is recorded as the target point cloud, and the real-time collected radar point cloud data after filtering is Denoted as the source point cloud, where X i ,Y i ,Z i is the 3D point coordinate data of the i-th point in the target point cloud, X j ,Y j ,Z j is the 3D point coordinate data of the jth point in the target point cloud; Find the nearest point set: traverse the target point cloud data, set the distance threshold, find the nearest point that corresponds to the source point cloud one by one, and record the set of points found in the target point cloud as The corresponding set of points in the source point cloud is Where P i represents the coordinates of the i-th 3D point in the target point cloud, S j Represents the 3D point coordinates of the jth point in the source point cloud; Centralized point cloud: Calculate the centroid of the nearest point set of the target point cloud and the corresponding point set of the source point cloud respectively, and centralize each point to obtain the target point cloud point set P relative to the centroid. i ′, source point cloud set S i ′, the calculation formula is as follows: S′ j =S j -c s (11); P i ′=P i -c p (12); Calculate the transformation matrix T of the source point cloud point set relative to the target point cloud point set, that is, the positioning error. The calculation formula is as follows: R=VU T (14); t=c p -Rc s (15); Among them, H represents the covariance matrix of the source point cloud and the target point cloud. H is decomposed into singular values ​​to obtain U, V, σ. The rotation transformation R and translation transformation t of the source point cloud point set relative to the target point cloud point set are calculated according to the decomposed matrix, and finally the transformation matrix T is obtained; among them, U is an orthogonal matrix composed of the left singular vectors of the matrix H; σ is a diagonal matrix, the diagonal elements are composed of the singular values ​​of the matrix H, the diagonal is the singular value, and the rest is 0; V T is the transpose of V, which is an orthogonal matrix consisting of the right singular vectors of the matrix H.

5. The method for closed-loop positioning and positioning data validity verification in a limited space based on global registration according to claim 4, characterized in that: The validity check of the corrected real-time positioning data specifically includes: Let the last moment positioning data output by the positioning correction module be D t-1 , the positioning data output at the current moment is D t , the current positioning data estimated by the validity verification module is recorded as D′ t , the time step is 1s, assuming that the frequency of obtaining the radar three-axis angular velocity and three-axis velocity is γ Hz, the x, y, z three-axis angular velocity obtained in real time within the time step of 1s is recorded as: The speeds of the three axes are recorded as: Then D is extracted by the following formulas: t-1 and D t The three-axis rotation angle and translation vector, assuming that the radar rotation order is ZYX, the calculation formula is as follows: θ Y =arctan(R 32 / R 33 ) (18); θ z =arctan(R 21 / R 11 ) (20); The three-axis angles and translation vectors at time t-1 and time t are respectively recorded as L t-1 , L t ; According to the following formula, the positioning data output at the current moment is D t Is it effective: ‖L t -L t-1 -L' t ‖≤L yuzhi (23); In formulas (21) to (23), the || operator represents the modulus of the vector, θ yuzhi is the artificially set allowable three-axis angle positioning error threshold, L yuzhi It is an artificially set allowable three-axis translation positioning error threshold.

6. A system for closed-loop positioning in limited space and verification of positioning data validity based on global registration, characterized in that: The system includes: a radar point cloud data real-time acquisition module, a point cloud data format conversion module, a three-dimensional point cloud map generation module, a positioning error calculation module, a real-time positioning correction module, and a positioning data validity verification module; The radar point cloud data real-time acquisition module is used to use a microcomputer and an open source program provided by a radar manufacturer to allow the microcomputer to communicate with the radar to obtain data from radar modules of different forms; The point cloud data format conversion module is used to convert the collected radar point cloud data into a standard input format of the three-dimensional point cloud map generation module; The three-dimensional point cloud map generation module is used to run an open source SLAM algorithm on a microcomputer, and to make the radar circle around the scene to be positioned by hand-held or other methods to obtain the three-dimensional point cloud map data of the scene; The positioning error calculation module is used to convert the real-time radar data and the three-dimensional map data into the same coordinate system according to the positioning data, radar point cloud data and three-dimensional point cloud map data output by the SLAM algorithm using the three-dimensional rigid body transformation technology, and perform noise reduction and voxel filtering. The processed data is subjected to point cloud registration operation, and the obtained transformation matrix is ​​recorded as the positioning error; The real-time positioning correction module is used to perform matrix multiplication on the positioning error obtained by the positioning error calculation module and the real-time positioning data output by the SLAM algorithm to obtain the corrected real-time positioning data; The positioning data validity verification module is used to calculate the estimated value of the current positioning data based on the three-axis speed and three-axis angular velocity of the radar in real-time operation, the positioning data output last time, and the time interval from the output of the last positioning data. The difference between the real-time positioning data output by the real-time positioning correction module and the estimated value is calculated, and the difference is compared with the set threshold to determine the radar positioning data of this time.

7. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 5 are implemented.

Images