A radar positioning method, device, storage medium and terminal

Abstract

The present invention provides a radar positioning method, device, storage medium and terminal. The method is applied to a movable device and includes: constructing an approximate nearest neighbor field based on the building floor plan of the environment; generating a fitting plane based on the first point cloud data of the environment; calculating an optimal solution of a first set of pose estimation values of the movable device based on the fitting plane, which includes a first coordinate axis change value, a roll angle change value and a pitch angle change value; filtering the first point cloud data to obtain second point cloud data; extracting line features and plane features from the second point cloud data; and matching the line features and the plane features with the approximate nearest neighbor field to obtain a second set of pose estimation values of the movable device, which includes a second coordinate axis change value, a third coordinate axis change value and a yaw angle change value. The present invention can significantly reduce the calculation amount and power consumption while ensuring the positioning accuracy, and can be widely applied to fields such as scientific research, entertainment services, construction, and logistics management.

Description

Technical Field

[0001] The present invention relates to the fields of computer vision and machine perception, and particularly to a radar positioning method, device, storage medium and terminal. Background Art

[0002] Regarding the problem of absolute indoor localization, some existing solutions rely on one or more passive wireless communication sensors to assist in positioning. By receiving the electromagnetic waves radiated or reflected by the target, the target position is detected, such as radio frequency identifiers, Wi-Fi, Bluetooth beacons, ultrasonic beacons, etc. These solutions have relatively large limitations, require professionals to enter the site in advance for layout and debugging, are time-consuming, laborious and expensive, and the service life of the hardware is limited. Another part of the solutions use known maps to assist in positioning. The core algorithms are based on Monte Carlo Localization, Stochastic Gradient Descent or Voronoi Segmentation. However, since the map needs to be discretely expressed in such methods and a large amount of information needs to be stored to achieve a certain accuracy, there is inevitably the disadvantage of large storage space requirements. Moreover, when performing the positioning task, a large amount of data needs to be processed, which not only consumes computing resources but also affects the real-time performance of the positioning. Summary of the Invention

[0003] In view of the above-mentioned disadvantages of the prior art, the purpose of the present invention is to provide a radar positioning method, device, storage medium and terminal for solving the problems in the prior art.

[0004] To achieve the above purpose and other related purposes, a first aspect of the present invention provides a radar positioning method applied to a movable device, including: constructing an approximate nearest neighbor field based on the building floor plan of the environment; generating a fitting plane based on the first point cloud data of the environment; calculating an optimal solution of a first set of pose estimation values of the movable device based on the fitting plane, which includes a first coordinate axis change value, a roll angle change value and a pitch angle change value, and the first coordinate axis is the gravity direction coordinate axis; filtering the first point cloud data to obtain second point cloud data; extracting line features and plane features from the second point cloud data; and matching the line features and the plane features with the approximate nearest neighbor field to obtain a second set of pose estimation values of the movable device, which includes a second coordinate axis change value, a third coordinate axis change value and a yaw angle change value.

[0005] In some embodiments of the first aspect of the present invention, the construction method of the approximate nearest neighbor field includes: extracting geometric elements in the building floor plan, and constructing a mathematical model, assigning weight values and numbers to them; dividing the building floor plan into multiple identical square regions; calculating the distances from the geometric elements in each square region to the center of the region to obtain a preset number of nearest geometric elements in each region; storing the numbers of the nearest geometric elements into the corresponding regions; repeating the process of dividing each square region into four smaller square regions until the numbers of the nearest geometric elements stored in the divided sub-regions are the same as those in the parent region before division or the division of the building floor plan reaches a preset number of layers.

[0006] In some embodiments of the first aspect of the present invention, the method for obtaining the second set of pose estimation values includes: projecting the line features and surface features onto any plane parallel to the fitted plane to obtain two-dimensional projection points; searching the approximate nearest neighbor field based on the coordinates of the two-dimensional projection points to match the nearest geometric elements; and optimizing the matching result by using a single-frame registration optimizer based on the weights of the matched nearest geometric elements to obtain the second set of pose estimation values.

[0007] In some embodiments of the first aspect of the present invention, the method includes: screening out a plurality of key frames from a preset number of point cloud frames continuously processed by the single-frame registration optimizer; and optimizing the matching results of the screened plurality of key frames by using a window registration optimizer to obtain the optimal solution of the second set of pose estimation values.

[0008] In some embodiments of the first aspect of the present invention, the method includes: the window registration optimizer uses a linear velocity model to constrain the motion between frames.

[0009] In some embodiments of the first aspect of the present invention, the screening method of the key frames includes: designing screening conditions based on any one or a combination of the time difference between frames, the motion displacement of the mobile device, and the angular change amount of the mobile device to obtain the key frames.

[0010] In some embodiments of the first aspect of the present invention, the method for obtaining the weight values includes: assigning weight values to the geometric elements based on the text or symbol information in the building floor plan; the text or symbol information includes door frame information, window information, fire pipeline information, and / or equipment instrument information.

[0011] To achieve the above and other related objectives, a second aspect of the present invention provides a radar positioning device, comprising: an approximate nearest neighbor field construction module for constructing an approximate nearest neighbor field based on the building floor plan of the environment; a radar module for collecting first point cloud data of the environment; a plane fitting module for generating a fitting plane based on the first point cloud data of the environment; a first group of pose estimation modules for calculating the optimal solution of the first group of pose estimation values of the movable device based on the fitting plane, including a first coordinate axis change value, a roll angle change value, and a pitch angle change value, where the first coordinate axis is the gravity direction coordinate axis; a filtering module for filtering the first point cloud data to obtain second point cloud data; a feature extraction module for extracting line features and surface features based on the second point cloud data; and a second group of pose estimation modules for matching the line features and the surface features with the approximate nearest neighbor field to obtain the second group of pose estimation values of the movable device, including a second coordinate axis change value, a third coordinate axis change value, and a yaw angle change value.

[0012] To achieve the above and other related objectives, a third aspect of the present invention provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the radar positioning method described above is implemented.

[0013] To achieve the above and other related objectives, a fourth aspect of the present invention provides an electronic terminal, comprising: a processor and a memory; the memory is used for storing a computer program, and the processor is used for executing the computer program stored in the memory so that the terminal executes the radar positioning method described above.

[0014] As described above, a radar positioning method, device, storage medium, and terminal proposed by the present invention have the following beneficial effects: By means of a building floor plan, an approximate nearest neighbor field (ANNF) is generated to provide a fast search method for the positioning task and ensure the real-time operation of the algorithm; The building floor plan has a small data volume and contains key information required to perceive the building structure to determine the specific pose (position and attitude). Therefore, the calculation amount can be significantly reduced while ensuring the positioning accuracy, the power consumption can be reduced, and it can be deployed on computing platforms with low computing power consumption and small power consumption, such as laptops, Raspberry Pi, and embedded systems, to improve its scope of application; The present invention can be widely applied to fields such as scientific research, entertainment services, construction, and logistics management. Specific cases include quality control management in construction, indoor positioning of catering and entertainment service robots or warehouse logistics robots, etc. Description of the Drawings

[0015] Figure 1 It shows a schematic flowchart of a radar positioning method in an embodiment of the present invention.

[0016] Figure 2 It shows a schematic flowchart of another radar positioning method in an embodiment of the present invention.

[0017] Figure 3 It shows a schematic flowchart of an optimized radar positioning method in an embodiment of the present invention.

[0018] Figure 4 It shows a schematic comparison diagram of the positioning effects of different algorithms in an embodiment of the present invention.

[0019] Figure 5 It shows a schematic structural diagram of a radar positioning device in an embodiment of the present invention.

[0020] Figure 6 It shows a schematic structural diagram of an electronic terminal in an embodiment of the present invention. Detailed implementation manners

[0021] The following uses specific specific examples to illustrate the implementation manners of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific implementation manners. Various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that, without conflict, the following embodiments and the features in the embodiments can be combined with each other.

[0022] Furthermore, as used herein, the singular forms "a", "an" and "the" are also intended to include the plural forms unless the context indicates otherwise. It should be further understood that the terms "comprising", "including" indicate the presence of the described features, operations, elements, components, items, types, and / or groups, but do not exclude the presence, appearance or addition of one or more other features, operations, elements, components, items, types, and / or groups. The terms "or" and "and / or" used herein are interpreted as inclusive, or meaning any one or any combination. Therefore, "A, B or C" or "A, B and / or C" means "any one of the following: A; B; C; A and B; A and C; B and C; A, B and C". An exception to this definition only occurs when the combination of elements, functions or operations is inherently mutually exclusive in some way.

[0023] The present invention provides a radar positioning algorithm, device, storage medium and terminal to solve the technical problems in the prior art that the positioning algorithm depends on passive sensors or has a large amount of calculation and high power consumption. The present invention is mainly described by taking indoor positioning as an example, but the present invention can also be applied to non-indoor environments, such as squares, open stadiums, etc.

[0024] It should be noted that the scenario to which the present invention is applied should be an artificial scenario, that is, a place with architectural floor plans. And, in the following embodiments, a mobile robot is taken as an example to illustrate the present invention, but the present invention is not limited to the application of mobile robots. For example, it can be applied to service robots in banks, hotels, and restaurants. Another example is that a complete set of equipment can be placed on a pushable tripod at a construction site, and the equipment can be manually pushed by an engineer to move.

[0025] In order to make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the technical solutions in the embodiments of the present invention will be further described in detail through the following embodiments in combination with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0026] Embodiment 1

[0027] As Figure 1 shown, this embodiment proposes a flow schematic diagram of a radar positioning method. The radar positioning method is applied to a mobile robot and includes steps S11 to S16, which are specifically described as follows:

[0028] Step S11. Construct an approximate nearest neighbor field based on the architectural floor plan of the environment.

[0029] In a preferred implementation manner of this embodiment, the construction method of the approximate nearest neighbor field includes: extracting geometric elements in the architectural floor plan, constructing a mathematical model, assigning weight values (greater than 0 and less than 1), and numbers to them; dividing the architectural floor plan into multiple identical square regions; calculating the distances from the geometric elements in each square region to the center of the region to obtain a preset number of nearest geometric elements in each region; storing the numbers of the nearest geometric elements in the corresponding regions; repeating the process of dividing each square region into four equal small square regions until the numbers of the nearest geometric elements stored in the divided sub-regions are the same as those in the parent region before division or the division of the architectural floor plan reaches a preset number of layers. After the approximate nearest neighbor field is constructed, discrete radar sampling points can be matched with the continuous architectural floor plan, providing a fast query interface for the later use of the single frame registration optimizer and the window optimization, so as to serve the radar positioning algorithm.

[0030] In some examples, the geometric elements within the square region include: unique geometric elements and common geometric elements. The unique geometric elements are completely contained within the square region, and only this square region among the square regions at the same division level needs to calculate the distance from the element to the center of the region. The common geometric elements pass through multiple square regions at the same division level, and it is necessary to calculate the distance from the element to the center of each corresponding region, that is, it is possible that the nearest elements in multiple regions are the same geometric element.

[0031] In a preferred implementation manner of this embodiment, the method for obtaining the nearest geometric element includes: searching for and storing the geometric elements of the square regions obtained by the first-level division; calculating the distance from each stored geometric element to the center of the region to obtain the N nearest geometric elements with the smallest distance; the sub-regions obtained by further division directly share the geometric elements stored in the parent region and calculate to obtain the N nearest geometric elements with the smallest distance in this sub-region.

[0032] In a preferred implementation manner of this embodiment, the method for obtaining the weight value includes: assigning weight values to the geometric elements based on the text or symbol information in the building floor plan; the text or symbol information includes door frame information, window information, fire pipeline information, and / or equipment instrument information.

[0033] In some examples, the method for constructing the approximate nearest neighborhood field includes: removing the useless layers in the electronic building floor plan, such as text annotations, pipelines, regions, etc., and only retaining geometric elements such as walls, square columns, cylindrical columns, and basic text or symbol information such as door frames, windows, fire pipelines, and equipment instruments; modeling the geometric elements with a continuous mathematical model and numbering them. For example, a common wall can be expressed as a two-dimensional finite line segment, a curved wall can be expressed as multiple two-dimensional finite line segments, a square column can be expressed as a square or four two-dimensional finite line segments, and a cylindrical column can be expressed as a hollow circular line segment; assigning weight values to the geometric elements based on the basic text or symbol information to serve the backend optimization; dividing the building floor plan into multiple square regions according to a preset length, ensuring that the square regions have the same side length at the beginning; calculating the distance from the geometric elements in each region to the center of the region and sorting them by distance (that is, the distance from the two-dimensional line segment, square line segment, and hollow circular line segment to the center point of the region), and each region will store the numbers of the N nearest elements; dividing each region into four small regions with equal side lengths, and repeating the above operations for each small region. If the numbers of the N nearest elements in the small region are the same as those in the parent region, stop the division operation; if they are different, continue to divide until they are the same or reach the deepest division level; if the numbers of the N nearest elements in the four small regions belonging to a parent region are all the same as those in the parent region, the operation of dividing the parent region into small regions can be withdrawn and the division can be terminated.

[0034] Step S12. Generate a fitting plane based on the first point cloud data of the environment. Specifically, the mobile robot obtains three-dimensional point cloud information of the surrounding environment through a radar (such as a three-dimensional lidar) at a certain frequency. Each time the radar scans, a frame of point cloud frame is obtained. The scanning frequency of the radar can be from several pulses per second to tens of thousands of pulses per second.

[0035] The mobile robot performs plane estimation based on the current point cloud frame to obtain a fitting plane. The fitting plane can be a ceiling plane, a ground plane, or two parallel ceiling planes and ground planes. Optional methods for the plane estimation include M-estimation, least squares method, etc. Among them, M-estimation is preferred because it has stronger robustness, and the corresponding obtained fitting plane has better robustness.

[0036] Step S13. Calculate the optimal solution of the first set of pose estimation values of the mobile robot based on the fitting plane, which includes the first coordinate axis change value, roll angle change value, and pitch angle change value. The first coordinate axis is the gravity direction coordinate axis. Specifically, based on the fitting plane, estimate the gravity axis direction information and the height information from the sensor to the ground, and then calculate to obtain the first coordinate axis change value of the mobile robot (in this embodiment, the movement in the Z-axis direction, that is, the height change of the robot, denoted as t z *), roll angle change value (i.e., the rotation angle along the X-axis, denoted as θ roll *), and pitch angle change value (i.e., the rotation angle along the Y-axis direction, denoted as θ pitch *).

[0037] In some examples, the method for initially establishing a coordinate system includes: defining the front direction of the radar as the X-axis direction, and defining the direction perpendicular to the radar and upward as the Z-axis direction. From this, the positive direction of the Y-axis can be deduced. Based on this coordinate system, calculate and obtain the optimal solution of the first set of pose estimation values of the mobile robot.

[0038] It should be noted that the optimal solution (also known as the final solution, optimal value, or final value) is the final output result, which will appear when calculating the first set of pose state quantities in plane fitting. In the subsequent figures, the optimal is denoted with a *. The estimated value, that is, the estimated solution, is a preliminary calculation result, which will be put into the next round of optimization, will appear after the single-frame registration optimizer optimizes, and is sent as an input to the window registration optimizer. In the subsequent figures, it is denoted without a *.

[0039] Step S14. Filter the first point cloud data to obtain the second point cloud data. Specifically, calculate the distances from all points in the first point cloud data to the fitting plane to obtain the point cloud data outside the preset distance range as the second point cloud data.

[0040] In some examples, the fitting planes are the ceiling plane and the ground plane which are parallel to each other. Based on a preset distance range, the point cloud data corresponding to the ceiling and the ground are removed from the first point cloud data to obtain the second point cloud data. In other examples, the fitting plane is the ground plane, and based on the preset distance range, the second point cloud data without the ground point cloud data can be obtained. The preset distance range includes the average sensing accuracy of the radar. For example, when the average sensing accuracy of the radar is 5 cm, the preset distance range can be set to 5 cm.

[0041] Step S15. Extract the line features and plane features from the second point cloud data. The optional feature extraction methods for the second point cloud data include: curvature extreme value method, broken line growth method, least square method, plane growth method, feature extraction based on the Random Sample Consensus (RANSAC) algorithm, straight line feature extraction based on the Hough Transform, feature extraction based on the polygon mesh model, feature extraction based on the point cloud model, and so on.

[0042] In a preferred implementation manner of this embodiment, the extraction process of the plane feature adopts a Voxel Grid Filter (also known as a voxelized grid filter). The Voxel Grid Filter can greatly reduce the number of points in the point cloud, reduce the sampling noise while maintaining the shape features of the point cloud, represent the sampling plane more accurately, and is beneficial to improving subsequent registration.

[0043] Step S16. Match the line features and the plane features with the approximate nearest neighbor field to obtain the second set of pose estimation values of the mobile robot, which includes the change value of the second coordinate axis (in this embodiment, the movement in the X-axis direction, denoted as t x *), the change value of the third coordinate axis (in this embodiment, the movement in the Y-axis direction, denoted as t y *), and the change value of the yaw angle (i.e., the rotation angle along the Z-axis direction, denoted as θ yaw *).

[0044] The search method for the approximate nearest neighbor field includes: inputting the coordinates of the search point in the map coordinate system to determine which square area the coordinate belongs to; continuously searching for the small area containing the point coordinates within this square area until the smallest area; outputting the N nearest elements of the smallest area, and this pair of input and output thus generated can be regarded as a set of matches, that is, the coordinates of the two-dimensional projection points obtained by sampling and processing and the geometric elements on the building floor plan form a match.

[0045] In a preferred embodiment of the present embodiment, the method for obtaining the second set of pose estimation values includes: projecting the line features and surface features onto any plane parallel to the fitting plane to obtain two-dimensional projection points (that is, projecting the remaining three-dimensional point clouds that have been compressed and labeled as line features or surface features onto a three-dimensional plane through the previously solved gravity axis direction, and performing a dimensionality reduction operation to obtain two-dimensional projection points. The three-dimensional plane can be the ground, the ceiling plane, or any plane parallel to the ground or the ceiling); searching for the approximate nearest neighbor field based on the coordinates of the two-dimensional projection points (the coordinates are transformed into coordinates in the map coordinate system) to match the nearest geometric elements (where the line features will be used to match the endpoints of the geometric elements in the building floor plan, and the surface features can directly match the corresponding geometric elements); based on the weights of the matched nearest geometric elements (the weights obtained based on basic text or symbol information), using a SingleFrame Registration Optimizer to optimize the matching result (inputting all the matches into the optimizer in sequence according to the weights) to obtain the second set of pose estimation values (t x , t y , θ yaw ). The SingleFrame Registration Optimizer is only used for single-point cloud input, that is, it only operates on the current frame.

[0046] In a preferred embodiment of the present embodiment, when the SingleFrame Registration Optimizer accumulates a certain number of frames, multiple key frames are integrated and packed and input into the window registration optimizer for a second round of optimization. Specifically, multiple key frames are selected from a preset number of point cloud frames continuously processed by the SingleFrame Registration Optimizer; furthermore, based on the window registration optimizer, the matching results of the selected multiple key frames are optimized to obtain the optimal solution of the second set of pose estimation values. Preferably, the window registration optimizer uses a linear velocity model to constrain the movement between frames.

[0047] Specifically, the input for the second-round optimization includes: the two-dimensional projection result of the point cloud input of a preset number of key frames that have been screened; and the second set of pose estimation value results obtained by the single-frame registration optimizer for the preset number of key frames. The process of the second-round optimization includes: calculating key frames, where the first frame of the radar readings must be a key frame, and then screening according to the screening method of key frames described above; when the number of key frames accumulates to K frames, input the second set of pose estimation values corresponding to the K key frames and the projected second point cloud data into the window registration optimizer for optimization, and the optimization result is the estimated values of the second set of poses corresponding to the K key frames after a new round of optimization; among them, the second set of pose estimation value of the earliest frame in time among the K key frames will be regarded as the optimal solution and output, and this frame will also be deleted from the key frames, and the remaining K-1 groups of data will be sent to the window registration optimizer again for optimization with the extraction of new key frames. Except for the first K-1 key frames at the beginning and the last K-1 key frames at the end, the remaining key frames in the middle positions will be optimized K times by the window registration optimizer before being officially output as the final result of the second set of pose estimation values, that is, the optimal solution.

[0048] In a preferred implementation manner of this embodiment, the screening method of the key frames includes: designing screening conditions based on any one or a combination of the time difference between frames, the movement displacement of the mobile robot, and the angle change amount of the mobile robot to obtain the key frames.

[0049] As Figure 2 shown, the flowchart of another radar positioning method proposed in the embodiment of the present invention is specifically described as follows: The radar inputs the collected three-dimensional point cloud data into the processor, and divides the fitting plane based on the three-dimensional point cloud data. Here, the ceiling plane and the ground plane are taken as examples. Based on the divided fitting planes, the gravity axis direction and the sensor height are estimated to obtain the optimal solutions of the three values required for pose estimation in the positioning problem: the height change value t z *, the roll angle change value θ roll *, and the pitch angle change value θ pitch *; Feature extraction is performed based on the input three-dimensional point cloud and the divided ceiling plane and ground plane, and the feature three-dimensional point cloud is projected onto the ground to obtain two-dimensional projection points; an approximate nearest neighbor field is constructed based on the input building floor plan, and the coordinates of each two-dimensional projection point are input into the approximate nearest neighbor field for search after conversion to match its nearest geometric element; all the matches are input into the single-frame registration optimizer in turn according to the weights for the first-round optimization, which will provide the other three values required for pose estimation in the radar positioning problem: the second coordinate axis change value t x 、the third coordinate axis change value t y and the yaw angle change value θ yaw; When the operations on the current frame accumulate to a certain number of frames, multiple key frames are integrated and packed and input into the window registration optimizer for the second-round optimization. This not only optimizes the matching problem between the two-dimensional projection points and the geometric elements in the building floor plan for each frame, but also adds a linear velocity model to constrain the movement between frames, so as to obtain the other three values t x 、t y and θ yaw for the final solution t x *, t y * and θ yaw *.

[0050] As Figure 3 shown, the embodiment of the present invention proposes a schematic diagram of the optimization process of a radar positioning method. The long box (FloorPlan, ANNF) represents the approximate nearest neighbor field, the node (hollow circle) represents the pose at the time of a single radar input, and the square box (v ij 、v jk 、v kl and v lm ) represents the linear velocity model. Among them, the connection between each node and the top long box represents the first-round optimization (high-frequency single-frame matching optimization, that is, the data obtained by each radar scan is processed and optimized); the whole represents the second-round optimization (low-frequency window matching optimization, that is, the results after single-frame matching optimization are screened by key frames and optimized. Among them, the solid-line hollow circles are the selected key frames and are given the English labels P i 、P j 、P k 、P l and P m ; the dotted-line hollow circles are discarded frames, that is, after the radar point cloud input is divided into planes, feature extraction, projection, and single-frame registration optimization, it is calculated that its pose does not meet the key frame standard, so it is discarded). The connection between each node above and the vertical line of the long box (whether dotted or solid) represents a single-frame matching optimization, and the connection between all the nodes below and the square boxes, between the square boxes, and the cooperation between all the solid-line nodes above and the vertical line of the long box represents a window matching optimization. In this appendix Figure 3 , K is 5.

[0051] As Figure 4 shown, the embodiment of the present invention proposes a comparison schematic diagram of the positioning effects of different algorithms. The radar positioning method (FP-Loc, Floor Plan Localization) proposed by the present invention is compared with the LeGO-LOAM algorithm and the ground truth (GT, Ground Truth). Among them, the thick line represents the positioning result of LeGO-LOAM, the medium line represents GT, and the thin line represents the positioning result of FP-Loc. From Figure 4It can be seen that the positioning result of the FP-Loc of the present invention is closer to the true value than that of the LeGO-LOAM algorithm, and the positioning is more accurate.

[0052] To verify the reliability of the present invention, Experiment 1 is specifically designed to demonstrate the stability and efficiency of the approximate nearest neighbor field ANNF, and Experiment 2 is designed to demonstrate the accuracy and real-time performance of the radar positioning algorithm.

[0053] In Experiment 1, by means of dense sampling and exhaustive search in the approximate nearest neighbor field, nearly 40 million coordinates are uniformly sampled within the searchable range, and the true value of the nearest element of the search coordinates themselves is obtained through exhaustive search, so as to compare the search output of the approximate nearest neighbor field. The comparison results are shown in Table 1. Table 1 can also be called the performance verification table of ANNF, which reflects the accuracy rate and search time under different division depths of the building floor plan. It can be seen from Table 1 that the deeper the division depth of the building floor plan, the higher the matching accuracy rate, and the search time does not change significantly.

[0054] Table 1. Performance verification table of ANNF

[0055]

[0056] In Experiment 2, by collecting the data fed back by the moving radar in the area of the known building floor plan and testing the accuracy of the positioning algorithm, the qualitative results can be referred to Figure 4 , and the quantitative results are shown in Table 2 and Table 3. Table 2 is the positioning result based on the unfurnished room dataset, in which the optimization results of the single-frame registration optimizer and the window registration optimizer are compared. Table 3 is the performance comparison result of different algorithms (FP-Loc and LeGO-LOAM) based on the corridor dataset. Both Table 2 and Table 3 use RPE (Relative Pose Error) and ATE (Absolute Trajectory Error) as performance indicators, and the smaller the value, the better the positioning effect.

[0057] It can be seen from Table 2 that under the condition that other conditions are the same, the positioning effect of this radar positioning method is better when the robot moves at a medium speed, and the second-round optimization (window registration optimizer) can effectively improve the registration accuracy of the first-round optimization (single-frame registration optimizer). It can be seen from Table 3 that under the condition that other conditions are the same, the RPE and ATE under the FP-Loc algorithm are significantly smaller than the RPE and ATE under the LeGO-LOAM algorithm on the whole, indicating that the positioning effect of the present invention is significantly better than that of the LeGO-LOAM algorithm.

[0058] Table 2. Positioning results based on the unfurnished room dataset

[0059]

[0060]

[0061] Table 3. Performance comparison results of FP-Loc and LeGO-LOAM based on the long corridor dataset

[0062]

[0063] In some embodiments, the method can be applied to a controller, such as an ARM (Advanced RISC Machines) controller, an FPGA (Field Programmable Gate Array) controller, a SoC (System on Chip) controller, a DSP (Digital Signal Processing) controller, or an MCU (Microcontroller Unit) controller, etc. In some embodiments, the method can also be applied to a computer including components such as a memory, a storage controller, one or more processing units (CPUs), a peripheral interface, an RF circuit, an audio circuit, a speaker, a microphone, an input / output (I / O) subsystem, a display screen, other output or control devices, and an external port; the computer includes, but is not limited to, personal computers such as desktop computers, laptop computers, tablet computers, smartphones, smart TVs, and personal digital assistants (PDAs for short). In other embodiments, the method can also be applied to a server, and the server can be arranged on one or more physical servers according to various factors such as function and load, or can be composed of a distributed or centralized server cluster.

[0064] Example Two

[0065] As Figure 5As shown in the figure, an embodiment of the present invention provides a schematic structural diagram of a radar positioning device, which includes: an approximate nearest neighbor field construction module 51 for constructing an approximate nearest neighbor field based on the building floor plan of the environment; a radar module 52 for collecting first point cloud data of the environment; a plane fitting module 53 for generating a fitting plane based on the first point cloud data of the environment; a first set of pose estimation modules 54 for calculating the optimal solution of the first set of pose estimation values of the mobile robot based on the fitting plane, which includes a first coordinate axis change value, a roll angle change value, and a pitch angle change value, and the first coordinate axis is the gravity direction coordinate axis; a filtering module 55 for filtering the first point cloud data to obtain second point cloud data; a feature extraction module 56 for extracting line features and surface features based on the second point cloud data; and a second set of pose estimation modules 57 for matching the line features and the surface features with the approximate nearest neighbor field to obtain the second set of pose estimation values of the mobile robot, which includes a second coordinate axis change value, a third coordinate axis change value, and a yaw angle change value.

[0066] It should be noted that the modules provided in this embodiment are similar to the methods and implementation manners provided above, so they will not be elaborated here. Additionally, it should be understood that the division of each module of the above device is only a logical function division. In actual implementation, they can be fully or partially integrated into a physical entity, or physically separated. And these modules can all be implemented in the form of software called by a processing element; they can also all be implemented in hardware; or some modules can be implemented in the form of software called by a processing element, and some modules can be implemented in hardware. For example, the first set of pose estimation modules 54 can be a separately established processing element, or can be integrated in a certain chip of the above device. In addition, it can also be stored in the memory of the above device in the form of program code, and called and executed by a certain processing element of the above device to perform the functions of the first set of pose estimation modules 54. The implementation of other modules is similar. In addition, these modules can be fully or partially integrated together or independently implemented. Here, the processing element can be an integrated circuit with signal processing capabilities. In the implementation process, each step of the above method or each of the above modules can be completed by the hardware integrated logic circuit or software-form instructions in the processor element.

[0067] For example, the above-mentioned modules may be one or more integrated circuits configured to implement the above methods, such as: one or more Application Specific Integrated Circuits (ASICs), or, one or more digital signal processors (DSPs), or, one or more Field Programmable Gate Arrays (FPGAs), etc. For another example, when a certain above-mentioned module is implemented in the form of a processing element scheduling program code, the processing element may be a general-purpose processor, such as a Central Processing Unit (CPU) or other processors that can call program code. For yet another example, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0068] Embodiment 3

[0069] An embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the radar positioning method described above is implemented.

[0070] Those of ordinary skill in the art can understand that all or part of the steps of implementing the above method embodiments can be completed by hardware related to a computer program. The foregoing computer program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps including the above method embodiments; and the foregoing storage medium includes: various media such as ROM, RAM, magnetic disk, or optical disc that can store program code.

[0071] Embodiment 4

[0072] As Figure 6 shown, an embodiment of the present invention provides a schematic structural diagram of an electronic terminal. The electronic terminal provided in this embodiment includes: a processor 61, a memory 62, and a communicator 63; the memory 62 is connected to the processor 61 and the communicator 63 through a system bus and completes communication therebetween. The memory 62 is used to store a computer program, the communicator 63 is used to communicate with other devices, and the processor 61 is used to run the computer program to enable the electronic terminal to execute each step of the radar positioning method described above.

[0073] The aforementioned system bus can be a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The system bus can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience in representation, only a thick line is used in the figure, but it does not mean that there is only one bus or one type of bus. The communication interface is used to implement communication between the database access device and other devices (such as clients, read-write libraries, and read-only libraries). The memory may include Random Access Memory (RAM), and may also include non-volatile memory, such as at least one disk memory.

[0074] The aforementioned processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it can also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0075] In summary, the present invention provides a radar positioning method, device, storage medium, and terminal. By means of a building floor plan, an approximate nearest neighbor field is generated to provide a fast search method for the positioning task and ensure the real-time operation of the algorithm; the building floor plan has a small data volume and contains key information required to sense the building structure to determine the specific pose (position and attitude). Therefore, the computational load can be significantly reduced and the power consumption can be lowered while ensuring the positioning accuracy; it can be deployed on computing platforms with low computing power consumption and small power consumption, such as laptops, Raspberry Pi, and embedded systems, to improve its scope of application; it can be widely used in fields such as scientific research, entertainment services, construction, and logistics management. Specific cases include quality control management in construction, indoor positioning of catering and entertainment service robots or warehouse logistics robots, etc. Therefore, the present invention effectively overcomes various drawbacks in the prior art and has high industrial utilization value.

[0076] The above embodiments are only illustrative of the principles and effects of the present invention and are not intended to limit the present invention. Any person familiar with this technology can modify or change the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by those with ordinary knowledge in the technical field without departing from the spirit and technical idea disclosed by the present invention should still be covered by the claims of the present invention.

Claims

1. A radar positioning method, characterized in that, applied to a movable device, including: Constructing an approximate nearest neighbor field based on the building floor plan of the environment; Generating a fitting plane based on the first point cloud data of the environment; Calculating the optimal solution of the first set of pose estimation values of the movable device based on the fitting plane, which includes the first coordinate axis change value, the roll angle change value, and the pitch angle change value, and the first coordinate axis is the gravity direction coordinate axis; Filtering the first point cloud data to obtain the second point cloud data; Extracting line features and surface features from the second point cloud data; Matching the line features and the surface features with the approximate nearest neighbor field to obtain the second set of pose estimation values of the movable device, which includes the second coordinate axis change value, the third coordinate axis change value, and the yaw angle change value; The construction method of the approximate nearest neighbor field includes: Extracting geometric elements in the building floor plan, and constructing a mathematical model, assigning weight values, and numbering them; Dividing the building floor plan into multiple identical square areas; Calculating the distance from the geometric elements in each square area to the center of the area to obtain a preset number of nearest geometric elements in each area; Storing the numbers of the nearest geometric elements into the corresponding areas; Repeating the process of dividing each square area into four equal small square areas until the numbers of the nearest geometric elements stored in the divided sub-areas are the same as those in the parent area before division or the division of the building floor plan reaches a preset number of layers; The obtaining method of the second set of pose estimation values includes: Projecting the line features and surface features onto any plane parallel to the fitting plane to obtain two-dimensional projection points; Searching the approximate nearest neighbor field based on the coordinates of the two-dimensional projection points to match the nearest geometric elements; Based on the weight values of the matched nearest geometric elements, using a single-frame registration optimizer to optimize the matching result to obtain the second set of pose estimation values.

2. The radar positioning method according to claim 1, characterized in that, including: Selecting multiple key frames from a preset number of point cloud frames continuously processed by the single-frame registration optimizer; Optimizing the matching results of the selected multiple key frames based on a window registration optimizer to obtain the optimal solution of the second set of pose estimation values.

3. The radar positioning method according to claim 2, characterized in that, including: The window registration optimizer uses a linear velocity model to constrain the movement between frames.

4. The radar positioning method according to claim 2, characterized in that, The screening method of the key frames includes: Designing screening conditions based on any one or a combination of the time difference between frames, the movement displacement of the movable device, and the angle change amount of the movable device to obtain the key frames.

5. The radar positioning method according to claim 1, characterized in that, The obtaining method of the weight values includes: Assigning weight values to the geometric elements based on the text or symbol information in the building floor plan; the text or symbol information includes door frame information, window information, fire pipeline information, and / or equipment instrument information.

6. A radar positioning device that implements the radar positioning method according to any one of claims 1-5, characterized in that, it includes: An approximate nearest neighborhood field construction module for constructing an approximate nearest neighborhood field based on the building floor plan of the environment; A radar module for collecting the first point cloud data of the environment; A plane fitting module for generating a fitting plane based on the first point cloud data of the environment; A first set of pose estimation modules for calculating the optimal solution of the first set of pose estimation values of the mobile device based on the fitting plane, which includes the first coordinate axis change value, the roll angle change value, and the pitch angle change value, and the first coordinate axis is the gravity direction coordinate axis; A filtering module for filtering the first point cloud data to obtain the second point cloud data; A feature extraction module for extracting line features and surface features based on the second point cloud data; A second set of pose estimation modules for matching the line features and the surface features with the approximate nearest neighborhood field to obtain the second set of pose estimation values of the mobile device, which includes the second coordinate axis change value, the third coordinate axis change value, and the yaw angle change value.

7. A computer-readable storage medium with a computer program stored thereon, characterized in that, when the computer program is executed by a processor, it implements the radar positioning method according to any one of claims 1 to 5.

8. An electronic terminal, characterized in that, it includes: A processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory so that the terminal executes the radar positioning method according to any one of claims 1 to 5.

Images