Ultra-wideband and lidar fusion indoor positioning method and related devices

Abstract

The application relates to an ultra-wideband and laser radar fusion indoor positioning method and related equipment, which comprises the following steps: acquiring UWB and LiDAR positioning trajectories; calculating reference trajectory coordinates; for a time point, the UWB and LiDAR positioning trajectory coordinates are fused through a plurality of fusion algorithms respectively, and the errors of the fusion coordinates of various fusion algorithms and the reference trajectory coordinates are calculated, so that the fusion coordinates with the minimum error are used as optimal estimation coordinates; the distances between the UWB, LiDAR positioning trajectory coordinates, the optimal estimation coordinates and the reference trajectory coordinates are calculated and compared respectively, so that the coordinates corresponding to the minimum distance are used as the fusion trajectory coordinates of the current time point; the above processing is sequentially carried out for all time points respectively, the segmented point fusion is realized, the fusion trajectory coordinates of all time points are obtained, and finally, the final fusion trajectory is obtained by combining the fusion trajectory coordinates of all time points and is output. The method is beneficial to improving the precision and robustness of indoor positioning and has low calculation complexity.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of indoor positioning, and in particular to an indoor positioning method based on fusion of ultra-wideband and laser radar and related equipment. BACKGROUND

[0002] In existing combined indoor positioning technology, most of them are combined with inertial measurement unit (IMU) and ultra-wideband technology (UWB), UWB and odometer (Odom), UWB and Wi-Fi, etc. for fusion. There are few studies on fusion of UWB and laser radar (LiDAR SLAM) for indoor positioning. Moreover, the fusion method is relatively single, and most of them are improved and fused based on traditional extended Kalman filter and particle filter.

[0003] UWB does not use the sine carrier in the traditional communication system, but uses nanosecond non-sine wave narrow pulse to transmit data. Even if wireless communication is used, the data transmission rate can reach hundreds of megabits per second or more. As one of the mainstream ways of current indoor positioning technology, UWB positioning has the advantages of high precision, low power consumption, strong penetration ability, etc. However, UWB positioning is usually applied in line of sight (LOS) environment. In the indoor scene with complex environment or obstacles, the positioning accuracy of UWB will be seriously affected. Laser radar SLAM technology is to construct a point cloud map by scanning the environment and complete indoor positioning, which can well compensate for the poor positioning accuracy of UWB in non-line of sight (NLOS) environment. It also has the advantages of high resolution and strong anti-interference ability. However, in the environment lacking of feature information, laser radar will produce cumulative error, and the positioning result may appear to deviate. At this time, UWB can be used to assist positioning. Selecting UWB+LiDAR sensor combination can complement the inevitable defects in single positioning method. In addition to obtaining trajectory information, it can also construct an environment map and provide more rich positioning information. However, to obtain ideal indoor positioning results, it is not enough to just select the combination of sensors. Further data processing is also needed. Data fusion algorithm is the key factor affecting the quality of combined positioning system.

[0004] The traditional extended Kalman filter algorithm (EKF) estimates the optimal state of the system through a recursive algorithm, which is derived from the standard Kalman filter algorithm, and the basic idea is to expand the nonlinear function into a Taylor series near the filter value and discard all high-order terms to realize linearization, and then use the traditional KF model to filter and complete state estimation. For EKF, its optimal estimate value will be directly affected by the process noise and observation noise, but the covariance of these two noises does not have a complete theoretical background in the parameter adjustment process and needs to be adjusted according to the actual situation, so it has great uncertainty. When the error covariance matrix of the LiDAR / UWB combined system cannot be accurately estimated, EKF will produce errors, resulting in inaccurate estimation results and further drift of the fused positioning trajectory. In addition, EKF will automatically ignore the quadratic and high-order terms of the Taylor expansion when dealing with nonlinear problems, so there will be a large linear error when the system is highly nonlinear. The traditional unscented Kalman filter algorithm (UKF) uses unscented transform (UT) to handle the nonlinear problem between the mean and the covariance. In the implementation process of UKF, the weights of sigma points and sampling points need to be initialized. The parameters need to be constantly adjusted to achieve excellent filtering performance, and the filtering process is also affected by noise, causing the estimation accuracy and robustness of UKF to decline. It can be seen that using a single fusion algorithm such as EKF or UKF cannot achieve ideal indoor positioning results. SUMMARY

[0005] The purpose of the present application is to provide an ultra-wideband and laser radar fusion indoor positioning method and related equipment, which is beneficial to improve the accuracy and robustness of indoor positioning, and has low computational complexity.

[0006] To achieve the above purpose, the technical scheme adopted by the present application is: an ultra-wideband and laser radar fusion indoor positioning method, comprising:

[0007] obtaining the positioning trajectory of UWB and the positioning trajectory of LiDAR;

[0008] calculating the reference trajectory coordinates;

[0009] For a time point, the positioning trajectory coordinates of UWB and the positioning trajectory coordinates of LiDAR are fused by several fusion algorithms respectively, and the errors of the fusion coordinates of various fusion algorithms and the reference trajectory coordinates are calculated, and the fusion coordinates of the fusion algorithm with the smallest error are taken as the optimal estimation coordinates;

[0010] The distances between the positioning trajectory coordinates of UWB, the positioning trajectory coordinates of LiDAR, the optimal estimation coordinates and the reference trajectory coordinates are calculated respectively and compared, and the coordinates corresponding to the smallest distance are taken as the fusion trajectory coordinates of the current time point.

[0011] The above processing is sequentially performed for all time points respectively to realize segmented point fusion, and the fusion trajectory coordinates of all time points are obtained;

[0012] The fusion trajectory coordinates of all time points are combined to obtain the final fusion trajectory and output.

[0013] Further, the calculation method of the reference trajectory is as follows:

[0014] The positioning trajectory coordinates of the UWB and the positioning trajectory coordinates of the LiDAR are fused by the KF algorithm to obtain the fusion coordinates of the KF algorithm;

[0015] The RMSE error RMSE uwb of the positioning trajectory coordinates of the UWB and the fusion coordinates of the KF algorithm is calculated respectively, and the RMSE error RMSE LiDAR of the positioning trajectory coordinates of the LiDAR and the fusion coordinates of the KF algorithm is calculated, according to the size of the RMSE uwb and the RMSE LiDAR , the weights ω1 and ω2 are set, and then the reference trajectory coordinates are calculated according to the following formula:

[0016]

[0017] , wherein P robot is the reference trajectory coordinates, P uwb is the positioning trajectory coordinates of the UWB, P LiDAR is the positioning trajectory coordinates of the LiDAR, ω1 is the weight of the positioning trajectory coordinates of the UWB, and ω2 is the weight of the positioning trajectory coordinates of the LiDAR.

[0018] Further, the plurality of fusion algorithms includes an EKF algorithm and a UKF algorithm.

[0019] Further, the error is an RMSE error.

[0020] Further, the distance is an Euclidean distance.

[0021] Further, the distances between the positioning trajectory coordinates of the UWB, the positioning trajectory coordinates of the LiDAR, the optimal estimation coordinates and the reference trajectory coordinates are calculated and compared, and the coordinates corresponding to the minimum distance are taken as the fusion trajectory coordinates of the current time point, and specifically:

[0022] The Euclidean distances between the positioning trajectory coordinates of the UWB, the positioning trajectory coordinates of the LiDAR, the optimal estimation coordinates and the reference trajectory coordinates are calculated, and specifically represented as follows:

[0023]

[0024] Where, x uwb y uwb x represents the coordinates of the UWB positioning trajectory. L y L These are the coordinates of the LiDAR positioning trajectory. For the optimal estimated coordinates, x ref y ref For the reference trajectory coordinates, d uwb Let d be the Euclidean distance between the UWB positioning trajectory coordinates and the reference trajectory coordinates. LiDAR This represents the Euclidean distance between the LiDAR positioning trajectory coordinates and the reference trajectory coordinates. The optimal estimated Euclidean distance between the coordinates and the reference trajectory coordinates;

[0025] With d uwb d LiDAR , The coordinates corresponding to the minimum of the three Euclidean distances are used as the fused trajectory coordinates at the current time point, as shown below:

[0026]

[0027] Where min[] represents taking the minimum value, X fus The coordinates of the fused trajectory at the current time point.

[0028] The present invention also provides an ultra-wideband and lidar fusion indoor positioning system, comprising:

[0029] The acquisition module is used to acquire the positioning trajectory of UWB and LiDAR, and to calculate the coordinates of the reference trajectory.

[0030] The fusion coordinate calculation and comparison module is used to fuse the positioning trajectory coordinates of UWB and LiDAR through several fusion algorithms, and calculate the error between the fused coordinates of each fusion algorithm and the reference trajectory coordinates. The fused coordinates of the fusion algorithm with the smallest error are taken as the optimal estimated coordinates.

[0031] The fusion trajectory coordinate calculation module is used to calculate and compare the distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, the optimal estimated coordinates, and the reference trajectory coordinates, and use the coordinates corresponding to the minimum distance as the fusion trajectory coordinates at the current time point.

[0032] The main process module is used to sequentially obtain the positioning trajectory coordinates of UWB, LiDAR, and reference trajectory coordinates at each time point from the acquisition module. It calls the fusion coordinate calculation and comparison module and the fusion trajectory coordinate calculation module to perform corresponding processing, realize segmented plotting and fusion, obtain the fusion trajectory coordinates of all time points, and then combine the fusion trajectory coordinates of all time points to obtain the final fusion trajectory and output it.

[0033] The application further provides an electronic device comprising a memory and a processor, wherein the memory stores a computer program capable of running on the processor, and the processor implements the above-mentioned ultra-wideband and laser radar fusion indoor positioning method when executing the computer program.

[0034] The application further provides a computer readable storage medium, which stores a computer program, and the computer program enables the processor to execute the above-mentioned ultra-wideband and laser radar fusion indoor positioning method when running on the processor.

[0035] Compared with the prior art, the application has the following beneficial effects: the application provides an improved ultra-wideband and laser radar fusion indoor positioning method and related equipment, the application can give full play to the respective advantages of UWB and laser radar sensors in a combined indoor positioning system, can complement each other in a complex environment, and provides a high-robustness and high-precision fusion scheme for indoor positioning. The application can autonomously select the currently optimal fusion algorithm according to the fusion results of UKF and EKF, and the fusion effect is better than that of a single EKF or UKF, the error is reduced, and the algorithm complexity is not increased. Therefore, the application has strong practicability and broad application prospect. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 is a method implementation flowchart of the embodiment of the application. DETAILED DESCRIPTION

[0037] The application will be further described below in combination with the drawings and embodiments.

[0038] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise indicated, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs.

[0039] It should be noted that the terms used herein are only for the purpose of describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, the singular form is intended to include the plural form unless the context clearly indicates otherwise, and furthermore, it should be understood that when the terms "comprise" and / or "include" are used in the specification, there is a presence of a feature, step, operation, device, component and / or combination thereof.

[0040] As shown in Figure 1 The embodiment provides an ultra-wideband and laser radar fusion indoor positioning method, which comprises the following steps:

[0041] S1, obtaining a positioning trajectory of UWB and a positioning trajectory of LiDAR.

[0042] S2, calculate the reference trajectory coordinates.

[0043] In this embodiment, the calculation method of the reference trajectory is:

[0044] The positioning trajectory coordinates of UWB and the positioning trajectory coordinates of LiDAR are fused by a traditional Kalman filtering algorithm (KF) to obtain the fusion coordinates of the KF algorithm.

[0045] The RMSE errors RMSE uwb of the positioning trajectory coordinates of UWB and the fusion coordinates of the KF algorithm are calculated respectively, and the RMSE errors RMSE LiSAR of the positioning trajectory coordinates of LiDAR and the fusion coordinates of the KF algorithm are calculated respectively, the weights ω1, ω2 are set according to the sizes of the RMSE uwb and the RMSE LiDAR , and then the reference trajectory coordinates are calculated according to the following formula:

[0046]

[0047] wherein, P robot is the reference trajectory coordinates, P uwb is the positioning trajectory coordinates of UWB, P LiDAR is the positioning trajectory coordinates of LiDAR, ω1 is the weight of the positioning trajectory coordinates of UWB, and ω2 is the weight of the positioning trajectory coordinates of LiDAR.

[0048] S3, for a time point, the positioning trajectory coordinates of UWB, the positioning trajectory coordinates of LiDAR and the reference trajectory coordinates at the current time point are obtained respectively, then the positioning trajectory coordinates of UWB and the positioning trajectory coordinates of LiDAR are fused by several fusion algorithms, and the errors of the fusion coordinates of various fusion algorithms and the reference trajectory coordinates are calculated, and the fusion coordinates of the fusion algorithm with the minimum error are taken as the optimal estimation coordinates.

[0049] In this embodiment, the positioning trajectory coordinates of UWB and the positioning trajectory coordinates of LiDAR are fused by EKF algorithm and UKF algorithm respectively, and then the RMSE errors of the fusion coordinates of EKF algorithm and UKF algorithm and the reference trajectory coordinates are calculated, and the fusion coordinates of the fusion algorithm with the minimum RMSE error are taken as the optimal estimation coordinates.

[0050] S4, the distances between the positioning trajectory coordinates of UWB, the positioning trajectory coordinates of LiDAR, the optimal estimation coordinates and the reference trajectory coordinates are calculated respectively and compared, and the coordinates corresponding to the minimum distance are taken as the fusion trajectory coordinates at the current time point.

[0051] In this embodiment, the Euclidean distances of the positioning trajectory coordinates of the UWB, the positioning trajectory coordinates of the LiDAR and the optimal estimation coordinates and the reference trajectory coordinates are calculated respectively, and are specifically expressed as follows:

[0052]

[0053] wherein x uwb , y uwb are the positioning trajectory coordinates of the UWB; x L , y L are the positioning trajectory coordinates of the LiDAR; is the optimal estimation coordinate, which can be the fusion coordinates (x ekf , y ekf ) obtained by the EKF algorithm, or the fusion coordinates (x ukf , y ukf ) obtained by the UKF algorithm; x ref , y ref are the reference trajectory coordinates; d uwb is the Euclidean distance of the positioning trajectory coordinates of the UWB and the reference trajectory coordinates; d LiDAR is the Euclidean distance of the positioning trajectory coordinates of the LiDAR and the reference trajectory coordinates; is the Euclidean distance of the optimal estimation coordinate and the reference trajectory coordinate.

[0054] The coordinate corresponding to the minimum value of the three Euclidean distances d uwb , d LiDAR , is taken as the fusion trajectory coordinate at the current time point, and is specifically expressed as follows:

[0055]

[0056] wherein min[] represents taking the minimum value, and X fus is the fusion trajectory coordinate at the current time point.

[0057] S5, for all time points t1, t2,..., t i , the processing of steps S2-S3 is sequentially performed until all time points are traversed, so as to realize the segmented point description fusion and obtain the fusion trajectory coordinates of all time points.

[0058] S6, the fusion trajectory coordinates of all time points are combined, so as to obtain the final fusion trajectory and output.

[0059] The embodiment also provides an indoor positioning system based on fusion of ultra-wideband and laser radar, which comprises an acquisition module, a fusion coordinate calculation and comparison module, a fusion trajectory coordinate calculation module and a main process module.

[0060] The acquisition module is configured to acquire the positioning trajectory of the UWB and the positioning trajectory of the LiDAR, and calculate reference trajectory coordinates.

[0061] The fusion coordinate calculation and comparison module is configured to fuse the positioning trajectory coordinates of the UWB and the LiDAR by using a plurality of fusion algorithms, and calculate the errors between the fusion coordinates of the various fusion algorithms and the reference trajectory coordinates, so that the fusion coordinates of the fusion algorithm with the minimum error are taken as the optimal estimation coordinates.

[0062] The fusion trajectory coordinate calculation module is configured to calculate the distances between the positioning trajectory coordinates of the UWB, the positioning trajectory coordinates of the LiDAR, the optimal estimation coordinates and the reference trajectory coordinates, and compare the distances, so that the coordinates corresponding to the minimum distance are taken as the fusion trajectory coordinates at the current time point.

[0063] The main process module is configured to sequentially acquire the positioning trajectory coordinates of the UWB, the positioning trajectory coordinates of the LiDAR and the reference trajectory coordinates at each time point from the acquisition module, call the fusion coordinate calculation and comparison module and the fusion trajectory coordinate calculation module to perform corresponding processing, realize segmented point fusion, obtain the fusion trajectory coordinates at all time points, and then combine the fusion trajectory coordinates at all time points to obtain a final fusion trajectory and output the final fusion trajectory.

[0064] The embodiment also provides an electronic device including a memory and a processor, wherein the memory stores a computer program capable of running on the processor, and the processor implements the above-mentioned ultra-wideband and laser radar fusion indoor positioning method when executing the computer program.

[0065] The embodiment also provides a computer readable storage medium, which stores a computer program, and the computer program causes the processor to execute the above-mentioned ultra-wideband and laser radar fusion indoor positioning method when running on the processor.

[0066] The application is applied to UWB / LiDAR combined positioning, the laser radar obtains the positioning trajectory of the laser radar by using the adaptive Monte Carlo method, constructs an environment map by using the Gmapping algorithm, and obtains the trajectory information of the LiDAR. After the UWB obtains the ranging information, the position is solved by using the trilateration method, and the trajectory information of the UWB is obtained. The two trajectory information are fused and compared by using the UKF and the EKF, the fusion algorithm with a small error is selected as the fusion algorithm at the current time point according to the RMSE of the reference trajectory, further optimization selection is performed, the segmented point fusion based on the Euclidean distance is realized, and thus the best trajectory estimation is obtained.

[0067] Those skilled in the art will appreciate that embodiments of the application can be readily used as software, hardware, or a combination of software and hardware. In one

[0068] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks.

[0069] one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks.

[0070] one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks.

[0071] one or more flowcharts and / or blocks in the flowcharts and / or a combination thereof. These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks.

Claims

1. A method for indoor positioning that combines ultra-wideband and lidar, characterized in that, include: Obtain the positioning trajectory of UWB and LiDAR; Calculate the coordinates of the reference trajectory; For the current time point, the positioning trajectory coordinates of UWB and LiDAR are fused using several fusion algorithms, and the error between the fused coordinates of each fusion algorithm and the reference trajectory coordinates is calculated. The fused coordinates of the fusion algorithm with the smallest error are taken as the optimal estimated coordinates. The distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, the optimal estimated coordinates, and the reference trajectory coordinates are calculated and compared. The coordinates corresponding to the minimum distance are taken as the fused trajectory coordinates at the current time point. The above processing is performed sequentially for all time points to achieve segmented point fusion and obtain the fused trajectory coordinates of all time points. By combining the fused trajectory coordinates of all time points, the final fused trajectory is obtained and output. The reference trajectory is calculated as follows: The KF algorithm is used to fuse the positioning trajectory coordinates of UWB and LiDAR to obtain the fused coordinates. Calculate the RMSE error between the UWB positioning trajectory coordinates and the fused coordinates from the KF algorithm. And the RMSE error between the LiDAR positioning trajectory coordinates and the fused coordinates of the KF algorithm. ,according to and The size is used to set the weight , Then calculate the reference trajectory coordinates using the following formula: in, For reference trajectory coordinates, These are the coordinates of the UWB positioning trajectory. These are the coordinates of the LiDAR positioning trajectory. The weights of the UWB positioning trajectory coordinates. The weights of the LiDAR positioning trajectory coordinates; The fusion algorithms include the EKF algorithm and the UKF algorithm; The process involves calculating and comparing the distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, and the optimal estimated coordinates with the reference trajectory coordinates, and using the coordinates corresponding to the minimum distance as the fused trajectory coordinates at the current time point. Specifically: The Euclidean distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, the optimal estimated coordinates, and the reference trajectory coordinates are calculated separately and expressed as follows: in, , These are the coordinates of the UWB positioning trajectory. , These are the coordinates of the LiDAR positioning trajectory. , For the optimal estimated coordinates, , For reference trajectory coordinates, This is the Euclidean distance between the UWB positioning trajectory coordinates and the reference trajectory coordinates. This represents the Euclidean distance between the LiDAR positioning trajectory coordinates and the reference trajectory coordinates. The optimal estimated Euclidean distance between the coordinates and the reference trajectory coordinates; by , , The coordinates corresponding to the minimum of the three Euclidean distances are used as the fused trajectory coordinates at the current time point, as shown below: in, This indicates taking the minimum value. The coordinates of the fused trajectory at the current time point.

2. The ultra-wideband and lidar fusion indoor positioning method according to claim 1, characterized in that, The error is the RMSE error.

3. A fusion indoor positioning system combining ultra-wideband and lidar, characterized in that, include: The acquisition module is used to acquire the positioning trajectory of UWB and LiDAR, and to calculate the coordinates of the reference trajectory. The fusion coordinate calculation and comparison module is used to fuse the positioning trajectory coordinates of UWB and LiDAR through several fusion algorithms, and calculate the error between the fused coordinates of each fusion algorithm and the reference trajectory coordinates. The fused coordinates of the fusion algorithm with the smallest error are taken as the optimal estimated coordinates. The fusion trajectory coordinate calculation module is used to calculate and compare the distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, the optimal estimated coordinates, and the reference trajectory coordinates, and use the coordinates corresponding to the minimum distance as the fusion trajectory coordinates at the current time point. The main process module is used to sequentially obtain the positioning trajectory coordinates of UWB, LiDAR, and reference trajectory coordinates at each time point from the acquisition module, call the fusion coordinate calculation and comparison module and the fusion trajectory coordinate calculation module to perform corresponding processing, realize segmented plotting and fusion, obtain the fusion trajectory coordinates of all time points, and then combine the fusion trajectory coordinates of all time points to obtain the final fusion trajectory and output it. The reference trajectory is calculated as follows: The KF algorithm is used to fuse the positioning trajectory coordinates of UWB and LiDAR to obtain the fused coordinates. Calculate the RMSE error between the UWB positioning trajectory coordinates and the fused coordinates from the KF algorithm. And the RMSE error between the LiDAR positioning trajectory coordinates and the fused coordinates of the KF algorithm. ,according to and The size is used to set the weight , Then calculate the reference trajectory coordinates using the following formula: in, For reference trajectory coordinates, These are the coordinates of the UWB positioning trajectory. These are the coordinates of the LiDAR positioning trajectory. The weights of the UWB positioning trajectory coordinates. The weights of the LiDAR positioning trajectory coordinates; The fusion algorithms include the EKF algorithm and the UKF algorithm; The process involves calculating and comparing the distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, and the optimal estimated coordinates with the reference trajectory coordinates, and using the coordinates corresponding to the minimum distance as the fused trajectory coordinates at the current time point. Specifically: The Euclidean distances between the UWB positioning trajectory coordinates, the LiDAR positioning trajectory coordinates, the optimal estimated coordinates, and the reference trajectory coordinates are calculated separately and expressed as follows: in, , These are the coordinates of the UWB positioning trajectory. , These are the coordinates of the LiDAR positioning trajectory. , For the optimal estimated coordinates, , For reference trajectory coordinates, This is the Euclidean distance between the UWB positioning trajectory coordinates and the reference trajectory coordinates. This represents the Euclidean distance between the LiDAR positioning trajectory coordinates and the reference trajectory coordinates. The optimal estimated Euclidean distance between the coordinates and the reference trajectory coordinates; by , , The coordinates corresponding to the minimum of the three Euclidean distances are used as the fused trajectory coordinates at the current time point, as shown below: in, This indicates taking the minimum value. The coordinates of the fused trajectory at the current time point.

4. An electronic device, characterized in that, It includes a memory and a processor, the memory storing a computer program that can run on the processor, and the processor executing the computer program to implement the ultra-wideband and lidar fusion indoor positioning method as described in claim 1 or 2.

5. A computer-readable storage medium, characterized in that, It stores a computer program that, when executed by a processor, causes the processor to perform the ultra-wideband and lidar fusion indoor positioning method as described in claim 1 or 2.

Images