Method and system for constructing and optimizing environment map based on ultra wide band (UWB)

Abstract

The invention provides a method and a system for constructing and optimizing an environment map based on an ultra wide band (UWB). The method and the system are suitable for an extreme environment. The method comprises the following steps: firstly, initializing a two-dimensional grid map by a system, and collecting signal data through UWB equipment; when the LOS path is determined, the corresponding grid is defined to be in a free state, and otherwise, the corresponding grid is marked to be in an occupied state. For NLOS features, obstacle locations are mapped onto a map. Then, the map is refined through a mean filter, a morphological filter and a region growing algorithm, and the accuracy of obstacle representation is improved; and finally, the optimized map information is utilized, and the weight of a positioning algorithm is adjusted through a weighted least square (WLS) method, so that the positioning precision is improved. The method has the advantages that environment obstacle information can be effectively constructed, the environment obstacle information is suitable for navigation after being refined, and a map can be normally constructed in dark light and smog environments. The system only depends on a UWB sensor, and accurate positioning is achieved in an obstacle environment.

Description

Technical Field

[0001] The present invention relates to a simultaneous localization and mapping (SLAM) technology, and in particular to a method and system for constructing and optimizing an environment map based on ultra-wideband (Ultra-Wideband, UWB). Background Art

[0002] Simultaneous localization and mapping (SLAM) is a complex technology that plays a vital role in the field of autonomous mobile robots and unmanned navigation. It requires the system to be able to build maps in unknown environments while accurately determining its real-time position. Currently, lidar systems and camera vision systems are common types of external sensors used for SLAM. However, the frequency range used by lidar and camera light sources is close to the visible light frequency range in the electromagnetic spectrum. Therefore, in challenging scenes such as those filled with smoke or containing a large number of mirrors, the signal-to-noise ratio of these optical measurement devices is relatively low, which makes SLAM algorithms that rely on such devices prone to failure, significantly reducing the functional performance of autonomous systems.

[0003] At present, navigation tasks in high temperature environments are a critical challenge. For example, in fire rescue operations, it is necessary to quickly locate trapped people through smoke and plan the safest evacuation route; in underground mines, high temperatures, dust and complex geological structures put forward higher requirements for technology; in the kitchen environment of the catering industry, especially those large commercial kitchens, continuous high temperatures and heavy oil smoke have a great impact on autonomous systems. In recent years, given the stability of radio frequency (RF) signals under harsh and extreme environmental conditions, there have been many studies exploring the use of RF bands instead of traditional dedicated sensors for SLAM. Some new RF devices such as ultra-wideband radar (UWB Radar) and micro-Doppler radar (Micro-Doppler Radar) have been able to initially complete SLAM tasks in challenging extreme environments, but their relatively high cost makes them difficult to be widely used in industrial production and experiments.

[0004] Due to the frequency characteristics of radio frequency signals and the relative economy of anchor-tag systems, robot positioning based on UWB systems has been widely studied. However, completing the mapping algorithm based on UWB systems remains a challenge.

[0005] Mapping based on UWB radar is classified into the category of feature point matching positioning and mapping. UWB radar transmits signals at specific time intervals and collects echo signals immediately. These echo signals are processed accordingly to calculate the time difference between the transmitted and received signals, and then the distance information is inferred based on the signal propagation speed. By using the triangulation positioning principle of Time of Arrival (TOA), two sets of such distance measurement values ​​can be used on a two-dimensional plane to preliminarily determine the coordinate position of the nearest obstacle. Then, the obstacles are effectively clustered into one category through the DBSCAN unsupervised clustering algorithm to obtain the obstacle entity. Then, as the movement continues, the position of the obstacle is continuously optimized, the entire algorithm process is completed, and the map is constructed. This method verifies the feasibility of UWB signal map creation in complex and harsh environmental conditions. However, UWB radar is different from the traditional UWB Anchor-Tag Systems communication system. It cannot be integrated in communication devices such as mobile phones and watches, and it is difficult to have a wider application; the use of UWB radar is expensive and the cost is high; UWB radar positioning still relies on point cloud feature point recognition for positioning, and the positioning accuracy is low.

[0006] The system integrates sonar and lidar to achieve map construction in visually obstructed environments. The system is equipped with a smoke sensor. When the smoke concentration in the environment exceeds the preset threshold, causing the efficiency of conventional optical sensing methods to decrease, the system can automatically switch to relying on sonar data for spatial perception and information acquisition. Under this strategy, the data detected by sonar is converted through the corresponding algorithm to supplement the point cloud information that is missing from the lidar due to smoke obstruction in an alternative form. This conversion process ensures that even when the direct observation capability of the lidar is blocked, it can still maintain effective perception and precise positioning of the environment, thereby continuously carrying out map construction operations. However, the sonar contains less information and still needs to rely on lidar data; at the same power, the sonar propagation distance is much smaller than that of the UWB signal, and the mapping range is small.

[0007] In extremely harsh indoor environments, such as smoke, dim light, and glass, the signal-to-noise ratio of traditional optical measurement equipment, such as lidar and cameras, is relatively low, which makes the SLAM algorithm that relies on such equipment prone to failure, significantly reducing the functional performance of the autonomous system and leading to mission failure. New sensor equipment such as UWB radar and sonar still rely on the algorithm framework of feature point matching, which makes the measurement information not fully called, the measurement range is small, the accuracy is low, and the system is expensive and costly.

[0008] Therefore, map construction in extremely harsh indoor environments requires: 1. Make full use of signal information to complete the construction of map information; 2. Be unaffected by harsh environments and have good robustness; 3. The equipment must be integrable and able to be used in large indoor scenes.

[0009] It should be noted that the information disclosed in the above background technology section is only used for understanding the background of the present application, and therefore may include information that does not constitute prior art known to ordinary technicians in the field. Summary of the invention

[0010] The main purpose of the present invention is to solve the problems existing in the above-mentioned background technology and to provide a method and system for constructing and optimizing an environmental map based on ultra-wideband (UWB).

[0011] To achieve the above object, the present invention adopts the following technical solutions:

[0012] In a first aspect of the present invention, a method for constructing and optimizing an environment map based on ultra-wideband (UWB) comprises the following steps:

[0013] S1. Map construction: Initialize a two-dimensional grid map and collect signal data in the environment through UWB devices; when there is a clear line of sight (LOS) path between the base station and the tag, define all grid cells on the path as "free"; if the communication result shows a non-LOS state, the relevant grid area is preliminarily classified as "occupied", assuming that there may be obstacles in the "occupied" area; in the known "free" grid cells, if non-line of sight (NLOS) features are found, the impact of the obstacle is mapped to the grid map to form a visual representation of the obstacle location;

[0014] S2. Map refinement: Binary filtering optimization is performed on the initially constructed map, including: applying a mean filter to remove noise and smooth the edges of obstacles to reduce the misjudgment of the initially marked "occupied" area; using a morphological filter to further verify and adjust the initially marked obstacle shape through dilation and erosion operations to ensure the accuracy of the obstacle boundary; applying a region growing algorithm to refine the obstacle boundary, merge adjacent similar areas, verify and adjust the initially marked "occupied" state, and achieve accurate representation of obstacles in the map;

[0015] S3. Map and positioning algorithm optimization: Utilize the optimized map information and adjust the positioning algorithm weights through the weighted least squares method (WLS) to reduce the adverse effects of NLOS data. Assign different weights to each ranging data point based on its NLOS recognition result, give lower weights to data that is more affected by NLOS, and give higher weights to LOS data to improve positioning accuracy. Thus, an optimized map and positioning results are obtained.

[0016] In a second aspect of the present invention, a system for constructing and optimizing an environment map based on ultra-wideband (UWB) includes:

[0017] The UWB positioning system includes a plurality of UWB base stations and at least one UWB tag carried on the mobile robot, which is used to measure the distance between the base station and the tag to provide raw data for positioning;

[0018] An IMU sensor, mounted on the mobile robot, is used to collect motion data of the mobile robot and assist UWB positioning;

[0019] A mobile robot that carries UWB tags and IMU sensors to move around the environment and take measurements;

[0020] The processing module communicates with the UWB base station and the UWB tag, and is used to execute the method of constructing and optimizing the environment map based on ultra-wideband UWB.

[0021] In a third aspect of the present invention, a computer-readable storage medium stores a computer program, wherein the computer program, when executed by a processor, implements the method for constructing and optimizing an environmental map based on ultra-wideband (UWB).

[0022] In a fourth aspect of the present invention, a computer program product includes a computer program, and when the computer program is executed by a processor, the method for constructing and optimizing an environment map based on ultra-wideband (UWB) is implemented.

[0023] The present invention has the following beneficial effects:

[0024] The present invention proposes a method and system for constructing and optimizing environmental maps based on ultra-wideband UWB, which can effectively construct and optimize environmental maps in extremely harsh indoor environments, such as smoke, dim light, or glass reflection interference, using ultra-wideband UWB technology. The method accurately constructs environmental obstacle information through communication between the base station and the tag, and through refinement processing, the map information can be used for subsequent navigation tasks. Even under adverse conditions such as dim light and smoke, the system can maintain the continuity and accuracy of map construction.

[0025] The present invention improves the accuracy and reliability of map construction by mapping UWB-based NLOS information on a two-dimensional grid map. The binary filtering algorithm used to correct the two-dimensional grid map constructed by UWB further improves the quality of the map. In addition, the prior grid map constructed in real time further corrects the positioning algorithm and optimizes the positioning accuracy.

[0026] The system uses only UWB as an external sensor and can perform accurate positioning even in an obstacle environment, showing good robustness. In addition, the present invention also implements a physical experiment of a Mecanum car model to verify the applicability and effectiveness of the algorithm in different environments. In general, the present invention provides a cost-effective, high-precision, highly adaptable and easy-to-integrate UWB map construction and optimization solution.

[0027] Other beneficial effects of the embodiments of the present invention will be further described below. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 This is a framework diagram of a mapping algorithm according to an embodiment of the present invention.

[0029] Figure 2 It is a visual representation diagram of the NLOS mapping algorithm according to an embodiment of the present invention.

[0030] Figure 3 Schematic diagram of the UWB mapping process according to an embodiment of the present invention.

[0031] Figure 4 This is a diagram of the physical process of UWB mapping according to an embodiment of the present invention.

[0032] Figure 5 This is a diagram of the actual result of UWB mapping according to an embodiment of the present invention.

[0033] Figure 6 This is a diagram showing the effect of multi-robot mapping in an embodiment of the present invention. DETAILED DESCRIPTION

[0034] The following is a detailed description of the embodiments of the present invention. It should be emphasized that the following description is only exemplary and is not intended to limit the scope and application of the present invention.

[0035] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0036] The present invention provides a method for constructing and optimizing an environmental map based on ultra-wideband (UWB), which mainly includes: 1. Mapping the judgment result based on NLOS information of UWB onto a two-dimensional grid map to complete map construction; 2. Correcting the two-dimensional grid map constructed by UWB using a binary filtering algorithm; 3. Further correcting the positioning algorithm using a priori grid map constructed in real time. The present invention realizes the construction of a map of an unknown environment in real time under extremely harsh environments. A physical experiment of a Mecanum car model is realized, which can be used for algorithm verification in different environments.

[0037] See also Figure 1 The embodiment of the present invention provides a method for constructing and optimizing an environment map based on ultra-wideband (UWB), comprising the following steps:

[0038] Step S1. Map construction: Initialize a two-dimensional grid map and collect signal data in the environment through UWB equipment; when there is a clear line-of-sight LOS path between the base station and the tag, define all grid cells on the path as "free"; if the communication result shows a non-LOS state, the relevant grid area is preliminarily classified as "occupied" and it is assumed that there may be obstacles in the "occupied" area; if NLOS features are found in the known "free" grid cells, the impact of the obstacle is mapped to the grid map to form a visual representation of the obstacle location.

[0039] In some embodiments, step S1 specifically includes:

[0040] Initialize the map: create a blank two-dimensional grid map, ready to record environmental information;

[0041] Data collection: Use UWB devices to collect signal data between base stations and tags in the environment;

[0042] LOS path identification: detects whether there is a LOS path between the base station and the tag, and determines whether the signal can propagate in a straight line;

[0043] Define free space: For the grid cells on the LOS path, mark them as "free", indicating that there are no obstacles in the area;

[0044] Non-LOS processing: For grid areas in non-LOS state, a conservative strategy is adopted to mark them as "occupied", indicating that there may be obstacles in the area;

[0045] NLOS feature mapping: For NLOS features detected in “free” grid cells, the actual location of obstacles is analyzed and inferred, and the location is visualized on the map;

[0046] Map state update: Based on the results of LOS path identification, non-LOS processing, and NLOS feature mapping, the grid state on the map is updated to reflect the current estimated position of obstacles in the environment.

[0047] Step S2. Map refinement: Binary filtering optimization is performed on the initially constructed map, including: applying a mean filter to remove noise and smooth the edges of obstacles to reduce the misjudgment of the initially marked “occupied” area; using a morphological filter to further verify and adjust the initially marked obstacle shape through dilation and erosion operations to ensure the accuracy of the obstacle boundary; applying a region growing algorithm to refine the obstacle boundary, merge adjacent similar areas, verify and adjust the initially marked “occupied” state, and achieve accurate representation of obstacles in the map;

[0048] In some embodiments, in step S2, the application of a mean filter to remove noise and smooth the edges of obstacles specifically includes: selecting an N×N window to calculate the neighborhood average of each pixel in the map; calculating the average value of all pixels within the N×N window; replacing the original value of the pixel in the center of the window with the calculated average value; repeating the above process for each pixel on the map until all pixels of the entire map are processed.

[0049] In some embodiments, in step S2, further verifying and adjusting the initially marked obstacle shape by dilation and erosion operations using a morphological filter specifically includes:

[0050] First, a dilation operation is applied around the initially identified obstacle boundaries to expand the marked obstacle area; then an erosion operation is applied to the map after the dilation operation to shrink the obstacle area and eliminate isolated noise points; the dilation operation is performed first and then the erosion operation is performed to fill small bright crack-like discontinuities.

[0051] In some embodiments, in step S2, the application of the region growing algorithm refines the obstacle boundary, merges adjacent similar regions, verifies and adjusts the "occupied" state of the preliminary mark, and realizes the accurate representation of obstacles in the map, specifically including: selecting pixels marked as obstacles in the map as seed pixels; performing similarity evaluation on adjacent pixels around the seed pixels to determine whether they meet the predetermined similarity criteria with the seed pixels; merging adjacent pixels that meet the similarity criteria into the obstacle region to which the seed pixels belong; performing iterative expansion, repeating the evaluation and merging operations until no more adjacent pixels that meet the conditions can be added to the obstacle region and the obstacle region reaches saturation.

[0052] Step S3. Map and positioning algorithm optimization: Utilize the optimized map information and adjust the positioning algorithm weights through the weighted least squares method (WLS) to reduce the adverse effects of non-line-of-sight (NLOS) data; assign different weights to each ranging data point according to its NLOS recognition result, give lower weights to data that is more affected by NLOS, and give higher weights to LOS data to improve positioning accuracy; thereby obtaining an optimized map and positioning result.

[0053] In some embodiments, step S3 specifically includes: assigning different weights to each data point according to the NLOS identification result of each ranging data point; wherein, a lower weight is assigned to the data point that is more affected by NLOS, and a higher weight is assigned to the LOS data point; constructing a weighted least squares (WLS) problem to optimize the positioning algorithm, and determining the position by solving the problem; using optimization techniques such as the Newton-Gaussian method or the Levenberg-Marquardt method to minimize the objective function; feeding back the occupancy information in the grid map to the positioning algorithm to enhance the accuracy and reliability of positioning; adjusting the weight according to the prior knowledge of obstacles and non-line-of-sight conditions in the grid map to adapt to the correction requirements of non-line-of-sight errors; using map information to achieve more accurate error suppression and performance optimization, and improve positioning accuracy.

[0054] In a preferred embodiment, for the constructed weighted least squares (WLS) problem, the weight of each ranging data point in the positioning process is dynamically adjusted according to the ratio of the total number of grids between the base station and the tag to the number of obstacle grids; an influencing factor is introduced to dynamically adjust the weight of the positioning algorithm according to the distribution of obstacles in the map and non-line-of-sight condition information to adapt to different environmental conditions.

[0055] An embodiment of the present invention also provides a system for constructing and optimizing an environmental map based on ultra-wideband (UWB), comprising: a UWB positioning system, comprising multiple UWB base stations and at least one UWB tag mounted on a mobile robot, for measuring the distance between the base station and the tag to provide raw data for positioning; an IMU sensor mounted on the mobile robot, for collecting motion data of the mobile robot to assist UWB positioning; a mobile robot, for carrying the UWB tag and the IMU sensor to move in the environment and perform measurements; a processing module, communicating with the UWB base station and the UWB tag, for executing the method for constructing and optimizing an environmental map based on ultra-wideband (UWB).

[0056] The present invention provides a system that can accurately construct and optimize environmental maps in extremely harsh indoor environments, such as smoke, dim light or glass. Using ultra-wideband (UWB) technology, the system effectively constructs environmental obstacle information through communication between a base station and a tag, and makes the map information suitable for subsequent navigation tasks through refinement processing. Even under adverse conditions such as dim light and smoke, the system can maintain the continuity and accuracy of map construction. The method improves the accuracy and reliability of map construction by mapping UWB non-line-of-sight (NLOS) information onto a two-dimensional grid map. The binary filtering algorithm used further optimizes the map and improves the accuracy of obstacle representation. In addition, the prior grid map constructed in real time further modifies the positioning algorithm and optimizes the positioning accuracy. The system of the present invention only uses UWB as an external sensor, and can complete accurate positioning functions even in an obstacle environment, showing good robustness. In addition, the system has been physically tested on a Mecanum car model to verify the applicability and effectiveness of the algorithm in different environments. In general, the present invention provides a cost-effective, accurate, adaptable and easy-to-integrate UWB map construction and optimization solution, which can achieve efficient and reliable map construction and positioning in a variety of complex environments.

[0057] The following further describes specific embodiments of the present invention and examples of algorithm implementation thereof.

[0058] System composition

[0059] Software system composition

[0060] The software system runs on the ubuntu18.04 (Linux operating system mainly for desktop applications) operating system and ROS-melodic (open source robot operating system) software environment.

[0061] ROS Melodic greatly simplifies the development, testing and deployment of robot software with its powerful modular architecture, rich software package collection and flexible programming interface. It provides a series of plug-and-play tools and libraries including sensor data processing, motion control, path planning, visual recognition, etc., allowing developers to quickly build and iterate complex robot functions. Through seamless integration with Ubuntu 18.04, ROS Melodic can make full use of the resource management, network support and graphical interface provided by the operating system to create a highly integrated and efficient development environment for robot researchers and engineers.

[0062] Hardware system composition

[0063] The experiment uses a four-wheeled mobile robot RIKEBOT. RIKEBOT is a wheeled robot equipped with four omnidirectional Mecanum wheels, a LeTMC-520RGBD depth camera, a RP-LiDAR 2D laser radar, a NoopLoop:LinkTrack UWB device, an MPU6050 IMU sensor, and computing hardware. JetsonNano TM , with a quad-core ARM processor and 16GB of memory. On the robot, a bracket is installed to ensure that there are no obstacles in the robot's own structure that block the UWB signal. A tag is installed on the robot's bracket at the same height as the four base stations in the environment.

[0064] UWB

[0065] Nooploop's LinkTrack UWB is an advanced positioning system based on ultra-wideband (UWB) technology. It provides installation support on a variety of operating systems including Ubuntu, allowing users to quickly build and configure the system according to actual needs. In addition to hardware devices, Nooploop may also provide supporting software tools and algorithms to help users achieve higher-level positioning services, such as trajectory tracking, environmental mapping and data analysis.

[0066] Visualization Platform

[0067] The mapping algorithm uses RVIZ as the visualization platform. RVIZ is a powerful 3D visualization tool in the Robot Operating System (ROS), which is mainly used to display and debug various data and status information in the robot system. It is an open platform built on the Qt framework, which aims to provide users with an intuitive way to observe and understand the dynamic changes of robots and their environment. RVIZ is an indispensable part of the ROS ecosystem. It greatly facilitates the visualization of complex information in the process of robot development, testing and maintenance, and is a powerful auxiliary tool for robot developers and researchers.

[0068] Range-SLAM is based on the UWB mapping algorithm framework and runs on the Ubuntu 18.04 operating system and ROS-melodic software environment. It is compiled and run on the main control machine of the hardware control system. Figure 2 The function includes constructing the surrounding environment, refining the obstacle information, and completing the environmental positioning under obstacles.

[0069] Mapping Algorithm

[0070] A method for building an environmental map using UWB is proposed, which mainly includes three parts: the first part realizes the correspondence between non-line-of-sight propagation and obstacles in the two-dimensional map; the second part uses an efficient binary filtering algorithm to further refine obstacles in the map; the third part further optimizes positioning through the constructed map to achieve a closed loop of simultaneous positioning and mapping. The process framework of the mapping algorithm is as follows Figure 1 shown.

[0071] 1. NLOS Mapping

[0072] In the UWB mapping system, when there is a clear line of sight (LOS) path between the base station and the tag, it means that there are no obstacles blocking the spatial transmission channel between the two, and the signal can propagate in a straight line without interference. Based on this ideal situation, all grid cells projected by the light along this path can be defined as "free" and recorded as {M line} LOS =C free , these grids usually represent open, unobstructed areas of space.

[0073] Similarly, if the communication result between the base station and the tag shows that it is not in the LOS state, it means that there is some form of physical obstacle on the expected path, which hinders the direct propagation of the signal. At this time, for such a non-line-of-sight NLOS scenario, the relevant grid area needs to be processed more carefully.

[0074] First, when facing grid areas that have not been fully detected or have unclear information, if they are initially judged to be in NLOS state, in order to ensure the robustness and security of the system, a conservative strategy is generally adopted to temporarily classify these grids as "occupied" state, that is, {M line} NLOS =C occupy , assuming that there may be potential obstacles in these areas that affect signal propagation.

[0075] Secondly, if a certain area in a known "free" grid cell is found to actually exhibit NLOS characteristics, this does not mean that the grid itself contains physical obstacles, but rather that there are obstacles on its path outward to the base station or tag. In this case, the core concept of the mapping framework is reflected - by converting complex NLOS identification results into intuitive geospatial expressions, that is, mapping the impact of obstacles onto the grid map, forming a visual representation of the obstacle location.

[0076] The specific expression can be expressed in mathematical formula as formula (4.1)

[0077]

[0078] In summary, this framework cleverly uses NLOS information to indirectly infer the location of actual obstacles, and displays these insights by occupying or releasing grid cells, so that the originally abstract radio wave propagation environment is transformed into an easy-to-understand and easy-to-operate grid map model, greatly enhancing the understanding and optimization of environmental perception and positioning performance.

[0079] Figure 2 A visual representation of the NLOS mapping algorithm is shown.

[0080] 2. Binary Filtering Optimization Map

[0081] In the initial application of the NLOS mapping algorithm, an unoptimized original map was generated. However, this map has several inherent problems: First, the initially constructed map often has noise in the data collected by the sensor, which may lead to inaccurate identification of the occupancy state of some areas; for example, there may be temporary misidentification or missing detection. Second, due to the limitations of the sensor resolution accuracy, especially in the edge areas of objects, the map may show fuzzy transition boundaries, for example, the edge of an object may be partially incorrectly marked as navigable space.

[0082] In order to further enhance and optimize the original map, an adaptive binary filtering technique is implemented. Binary filter is an image processing technique mainly used to perform binary filtering operations on images. The main function of this filter is to extract specific structural information of obstacles in the image while eliminating the noise components. In this paper, the binary filter consists of three parts: mean filter, morphological filter and region growing algorithm.

[0083] Mean filter

[0084] The implementation method of the mean filter is to calculate the average value of all pixels in the N×N window, and then replace the original value of the central pixel with the average value calculated by the window, see formula (4.2)

[0085]

[0086] This approach is effective for removing low frequency noise and constitutes a straightforward linear filtering technique.

[0087] Morphological filter

[0088] The dilation operation is used to expand the obstacle area, and the erosion operation is used to shrink the obstacle area and eliminate isolated noise points. After using the average filter to remove small black point noise, the closing operation is applied, that is, dilation followed by erosion, to fill small bright crack-like discontinuities. Specifically, equations (4.3) and (4.4) represent the erosion and dilation operations, respectively:

[0089]

[0090] Region Growing Algorithm

[0091] In the implementation of the region growing algorithm, adjacent pixels are checked in detail to see if they meet the predefined similarity criteria. Once these pixels meet the criteria, they are merged into the region currently being constructed. Subsequently, the iterative process of evaluation and expansion continues, applying the same review and growth steps to the newly included pixels until the entire region reaches saturation, which means that no more qualified new pixels can be found, at which point the algorithm naturally ends. In general, region growing algorithms are mostly applied to grayscale images. Here, adjustments are made so that it can be applied to filtering in binary images, thereby better obtaining the required obstacle area. The specific formula is shown in formula (4.5):

[0092]

[0093] Where R represents the area occupied by the obstacle in the current map, and N represents a set of neighboring nodes around R. The function d represents the degree of similarity between the region R and the grid point. T represents the threshold. If the similarity measure is less than the threshold, the grid point becomes occupied and becomes part of the obstacle region R. Here, the improved region growing algorithm applied to binary images shows significant effectiveness in optimizing the obstacle region segmentation.

[0094] 3. Optimization of UWB Positioning Using Priori Maps

[0095] A positioning method based on weighted least squares (WLS) is introduced to mitigate the adverse effects of non-line-of-sight NLOS data. Its characteristic is that different weights are assigned to each ranging data point according to its NLOS recognition result, so that when solving the positioning problem, a lower weight is given to the data that is more affected by NLOS, while a higher weight is given to the line-of-sight LOS data. This processing method is expected to make the positioning algorithm more adaptable and robust in the face of complex and changeable non-line-of-sight environments, thereby significantly improving the positioning accuracy.

[0096] The following WLS questions will be constructed:

[0097]

[0098] where β i is the weight of the ranging measurement, which is related to the score of the NLOS judgment measurement, and λ is a constant coefficient. Minimization is achieved by optimization techniques such as the Newton-Gauss method or the Levenberg-Marquardt method.

[0099] The occupancy grid map not only serves as the basis for environmental representation, but also its stored occupancy information will be actively fed back to the positioning link to enhance the accuracy and reliability of positioning. When combined with the map information obtained by UWB, the weight β in the original positioning algorithm is i The setting of will be changed to adapt to the correction requirements for non-line-of-sight errors. Formula (3.8) will be revised to formula (3.9) to better reflect and utilize the prior knowledge about obstacles and non-line-of-sight conditions in the environment map, so as to achieve more accurate error suppression and performance optimization in the actual positioning process.

[0100]

[0101] Where N Total,i and N Occupy,i They represent the total number of grids and the number of obstacle grids passed on the connecting line between base station i and the tag, respectively, and ζ∈[0,1] indicates the degree to which the positioning is affected by the map.

[0102] Algorithm 1: Range-SLAM pseudo code is as follows:

[0103]

[0104] See also Figures 3 to 6 , Figure 3 The UWB mapping process of an embodiment of the present invention is shown. Figure 4 The actual process of UWB mapping in an embodiment of the present invention is shown. Figure 5 The actual result of UWB mapping of the embodiment of the present invention is shown. Figure 6 The multi-robot mapping effect of an embodiment of the present invention is demonstrated.

[0105] Compared with the prior art, the main advantages of UWB map construction and optimization by the present invention are:

[0106] 1. Through the communication between the base station and the tag, the environmental obstacle information can be constructed normally;

[0107] 2. The obstacle information of the map has been refined and can be applied to subsequent navigation;

[0108] 3. In extreme dark light and smoke conditions, it does not affect the construction of the map.

[0109] 4. The system only uses UWB as an external sensor and can still complete the positioning function well in an obstacle environment.

[0110] An embodiment of the present invention further provides a storage medium for storing a computer program, which at least performs the above method when executed.

[0111] An embodiment of the present invention further provides a control device, comprising a processor and a storage medium for storing a computer program; wherein the processor is configured to execute at least the method described above when executing the computer program.

[0112] An embodiment of the present invention further provides a processor, wherein the processor executes a computer program and at least executes the method described above.

[0113] The storage medium can be implemented by any type of non-volatile storage device, or a combination thereof. Among them, the non-volatile memory can be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a magnetic random access memory (FRAM), a ferromagnetic random access memory (Flash Memory), a magnetic surface memory, an optical disc, or a compact disc read-only memory (CD-ROM); the magnetic surface memory can be a disk memory or a tape memory. The storage medium described in the embodiments of the present invention is intended to include, but is not limited to, these and any other suitable types of memory.

[0114] In the several embodiments provided by the present invention, it should be understood that the disclosed systems and methods can be implemented in other ways. The device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as: multiple units or components can be combined, or can be integrated into another system, or some features can be ignored or not executed. In addition, the coupling, direct coupling, or communication connection between the components shown or discussed can be through some interfaces, and the indirect coupling or communication connection of the devices or units can be electrical, mechanical or other forms.

[0115] The units described above as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units; some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0116] In addition, all functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may be separately used as a unit, or two or more units may be integrated into one unit; the above-mentioned integrated units may be implemented in the form of hardware or in the form of hardware plus software functional units.

[0117] A person skilled in the art can understand that: all or part of the steps of implementing the above method embodiment can be completed by hardware related to program instructions, and the aforementioned program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps of the above method embodiment; and the aforementioned storage medium includes: mobile storage devices, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), disks or optical disks, etc. Various media that can store program codes.

[0118] Alternatively, if the above-mentioned integrated unit of the present invention is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present invention can be essentially or partly reflected in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as mobile storage devices, ROM, RAM, magnetic disks or optical disks.

[0119] The methods disclosed in the several method embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method embodiments.

[0120] The features disclosed in several product embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new product embodiments.

[0121] The features disclosed in several method or device embodiments provided by the present invention can be arbitrarily combined without conflict to obtain new method embodiments or device embodiments.

[0122] The above contents are further detailed descriptions of the present invention in combination with specific preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art of the present invention, several equivalent substitutions or obvious variations can be made without departing from the concept of the present invention, and the performance or use is the same, which should be regarded as belonging to the protection scope of the present invention.

Claims

1. A method for constructing and optimizing an environment map based on ultra-wideband (UWB), characterized in that: The following steps are involved: S1. Map construction: Initialize a two-dimensional grid map and collect signal data in the environment through UWB devices; when there is a clear line-of-sight LOS path between the base station and the tag, define all grid cells on the path as "free"; if the communication result shows a non-LOS state, the relevant grid area is preliminarily classified as "occupied", assuming that there may be obstacles in the "occupied" area; in the known "free" grid cells, if non-line-of-sight NLOS features are found, the impact of the obstacle is mapped to the grid map to form a visual representation of the obstacle location; S2. Map refinement: Binary filtering optimization is performed on the initially constructed map, including: applying a mean filter to remove noise and smooth the edges of obstacles to reduce the misjudgment of areas initially marked as "occupied"; Use morphological filters to further verify and adjust the shapes of obstacles marked initially through dilation and erosion operations to ensure the accuracy of obstacle boundaries; apply region growing algorithms to refine obstacle boundaries, merge adjacent similar regions, verify and adjust the "occupancy" status of initial markings, and achieve accurate representation of obstacles in the map; S3. Map and positioning algorithm optimization: Utilize the optimized map information and adjust the positioning algorithm weights through the weighted least squares method (WLS) to reduce the adverse effects of NLOS data. Assign different weights to each ranging data point based on its NLOS recognition result, give lower weights to data that is more affected by NLOS, and give higher weights to LOS data to improve positioning accuracy. Thus, an optimized map and positioning results are obtained.

2. The method for constructing and optimizing an environment map based on ultra-wideband (UWB) as claimed in claim 1, characterized in that: Step S1 specifically includes: Create a blank 2D grid map ready for recording environmental information; Use UWB devices to collect signal data between base stations and tags in the environment; Detect whether there is a LOS path between the base station and the tag to determine whether the signal can propagate in a straight line; For the grid cells on the LOS path, they are marked as "free", indicating that there are no obstacles in the area; For the grid area in non-LOS state, a conservative strategy is adopted to mark it as "occupied", indicating that there may be obstacles in the area; For NLOS features detected in "free" grid cells, analyze and infer the actual location of the obstacle and visualize the location on the map; Based on the results of LOS path identification, non-LOS processing, and NLOS feature mapping, the grid state on the map is updated to reflect the current estimated positions of obstacles in the environment.

3. The method for constructing and optimizing an environment map based on ultra-wideband (UWB) as claimed in claim 1, characterized in that: In step S2, the application of a mean filter to remove noise and smooth the obstacle edge specifically includes: Select an N×N window to calculate the neighborhood average of each pixel in the map; Calculate the average value of all pixels in the N×N window; Replace the original value of the center pixel of the window with the calculated average value; The above process is repeated for each pixel on the map until all pixels of the entire map have been processed.

4. The method for constructing and optimizing an environment map based on ultra-wideband (UWB) as claimed in claim 1, characterized in that: In step S2, the use of a morphological filter to further verify and adjust the shape of the initially marked obstacle through dilation and erosion operations specifically includes: First, a dilation operation is applied around the initially identified obstacle boundaries to expand the marked obstacle area; then an erosion operation is applied to the map after the dilation operation to shrink the obstacle area and eliminate isolated noise points; the dilation operation is performed first and then the erosion operation is performed to fill small bright crack-like discontinuities.

5. The method for constructing and optimizing an environment map based on ultra-wideband (UWB) as claimed in claim 1, characterized in that: In step S2, the application of the region growing algorithm refines the obstacle boundary, merges adjacent similar regions, verifies and adjusts the "occupied" state of the preliminary mark, and realizes the accurate representation of obstacles in the map, which specifically includes: Select pixels marked as obstacles in the map as seed pixels; Performing similarity evaluation on adjacent pixels around the seed pixel to determine whether they meet a predetermined similarity standard with the seed pixel; Merge adjacent pixels that meet the similarity criteria into the obstacle region to which the seed pixel belongs; Iterative expansion, repeated evaluation and merging operations are performed until no more eligible adjacent pixels can be added to the obstacle area and the obstacle area reaches saturation.

6. The method for constructing and optimizing an environment map based on ultra-wideband (UWB) as claimed in claim 1, characterized in that: Step S3 specifically includes: According to the NLOS identification result of each ranging data point, different weights are assigned to each data point; among which, a lower weight is assigned to the data point that is more affected by NLOS, while a higher weight is assigned to the LOS data point; A weighted least squares (WLS) problem is constructed to optimize the positioning algorithm, and the location is determined by solving the problem. Use optimization techniques such as the Newton-Gaussian method or the Levenberg-Marquardt method to minimize the objective function; Feed the occupancy information in the grid map back into the positioning algorithm to enhance the accuracy and reliability of positioning; The weights are adjusted based on the prior knowledge of obstacles and non-line-of-sight conditions in the grid map to meet the correction requirements of non-line-of-sight errors; Utilize map information to achieve more precise error suppression and performance optimization, and improve positioning accuracy.

7. The method for constructing and optimizing an environment map based on ultra-wideband (UWB) as claimed in claim 6, characterized in that: For the constructed weighted least squares (WLS) problem, the weight of each ranging data point in the positioning process is dynamically adjusted according to the ratio of the total number of grids between the base station and the tag to the number of obstacle grids; the influencing factor is introduced to dynamically adjust the weight of the positioning algorithm according to the distribution of obstacles and non-line-of-sight conditions in the map to adapt to different environmental conditions.

8. A system for constructing and optimizing environmental maps based on ultra-wideband (UWB), characterized in that: include: The UWB positioning system includes a plurality of UWB base stations and at least one UWB tag carried on the mobile robot, which is used to measure the distance between the base station and the tag to provide raw data for positioning; An IMU sensor, mounted on the mobile robot, is used to collect motion data of the mobile robot and assist UWB positioning; A mobile robot that carries UWB tags and IMU sensors to move around the environment and take measurements; The processing module communicates with the UWB base station and the UWB tag, and is used to execute the method for constructing and optimizing an environment map based on ultra-wideband UWB as described in claim 1.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method for constructing and optimizing an environment map based on ultra-wideband (UWB) as described in any one of claims 1 to 7 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the method for constructing and optimizing an environment map based on ultra-wideband (UWB) as described in any one of claims 1 to 7 is implemented.

Images