Single-sensor type robust high-precision three-dimensional positioning method and system

Abstract

The invention discloses a single-sensor type robust high-precision three-dimensional positioning method and system, and aims to improve the positioning precision and stability of a moving target in a three-dimensional scene. According to the method, a geometric constraint structure formed by a multi-label sensor is constructed, rich observation information is fused, and simultaneous acquisition of three-dimensional position and attitude information is realized. The optimal layout coordinates of a base station sensor and a label sensor are calculated by adopting a particle swarm optimization algorithm, and high-precision three-dimensional position and attitude calculation is realized by combining a weighted least square algorithm of single-point positioning, a weighted least square algorithm of rigid body positioning, robust M estimation, gross error detection and a Kalman filtering backward smoothing algorithm. According to the method, in a complex environment of non-line-of-sight signal interference, the positioning robustness can be remarkably enhanced, the elevation error is reduced, and the application feasibility and the precision reliability of a single type of sensor in an actual scene are improved.

Description

TECHNICAL FIELD

[0001] The application provides a single-sensor type robust high-precision three-dimensional positioning method and system, and belongs to the field of navigation and positioning. BACKGROUND

[0002] In environments where Global Navigation Satellite System (GNSS) signals are unavailable or limited, such as underground spaces, urban canyons and complex indoor areas, traditional satellite navigation is difficult to provide continuous and reliable positioning services. Therefore, Wireless Positioning Technologies (WPT) relying on ground infrastructure has become an effective alternative solution for high-precision positioning. Such technologies use the propagation characteristics of wireless signals such as Ultra-Wideband (UWB) and Bluetooth Low Energy (BLE), including Received Signal Strength Indicator (RSSI), Angle of Arrival (AOA), Time of Arrival (TOA) or Time Difference of Arrival (TDOA) information, to calculate the three-dimensional spatial position of the target device. With high precision and fast response capability, wireless positioning systems have been widely used in Industrial Internet of Things (IIoT), smart manufacturing, warehouse logistics, Autonomous Mobile Robots (AMR), smart healthcare and Real-Time Locating Systems (RTLS) fields, meeting the application requirements of centimeter-level positioning accuracy and low-latency communication.

[0003] Compared with GNSS, wireless positioning technology has the following advantages: 1. Flexible deployment: can operate independently in environments without satellite signal coverage; 2. High ranging accuracy: for example, UWB technology has a bandwidth of more than 500 MHz and nanosecond-level pulse signals, with good time resolution and multipath suppression capability; 3. Low power design: for example, BLE is suitable for wearable devices and other energy-sensitive scenarios; 4. Fast response speed: supports high-frequency, real-time continuous tracking of dynamic targets.

[0004] Despite this, such technology also faces many challenges in practical applications, one of the most critical issues is the non-line-of-sight (NLOS). In a complex indoor environment, wireless signals are often blocked by walls, equipment or human bodies, resulting in longer propagation paths and introducing systematic ranging errors. This error not only reduces the reliability of single ranging, but also may cause error divergence in the positioning solution process. Taking the UWB wireless positioning system based on ranging as an example, the system usually deploys multiple base station nodes and mobile tags, and uses multi-lateral ranging algorithms (such as TOA, TDOA) to solve the target position. Its performance is highly dependent on ranging accuracy, and the ranging deviation caused by NLOS signals often destroys the stability and robustness of the solution. Therefore, how to identify and suppress NLOS errors to improve the accuracy and robustness of the positioning system is one of the key research directions in current wireless positioning technology. SUMMARY

[0005] The application uses rigid body positioning algorithm and multi-level constraint optimization to propose a single sensor type robust high-precision three-dimensional positioning method and system, which can improve the accuracy and robustness of single type sensor three-dimensional positioning.

[0006] The technical scheme of the application is a single sensor type robust high-precision three-dimensional positioning method, comprising the following steps: Step 1, deploying sensors and completing distance observation calibration; Step 2, input distance observation at the same time , wherein i is the base station number, j is the tag number; the calibrated distance observation value is calculated according to the calibration parameters ; Step 3, selecting different positioning algorithms according to the number of label sensors, and marking the positioning algorithms; Step 4, setting the iteration number equal to 0, selecting different weighted least squares to solve three-dimensional attitude and position initial values and residuals according to the positioning algorithm mark; Step 5, judge whether to continue iteration: when the iteration number is greater than the threshold value or the residual is less than the threshold value , stop iteration and execute step 9; otherwise, execute step 6; Step 6, according to the algorithm type set in step 3, calculate the three-dimensional attitude and position iteration value by different iteration method, and add 1 to the iteration number ; Step 7, update the observation weight matrix using the robust M estimation of residual absolute value and minimum criterion, which is used for step 6 iteration value calculation; Step 8, the rough error elimination method of w-test hypothesis test eliminates the observation containing rough error Then step 5 is performed; Step 9, the positioning result of the previous time is obtained by the Kalman filter backward smoothing algorithm.

[0007] Further, the specific implementation of step 1 is as follows: Step 1.1, set the sensor related parameters , including the three-dimensional coordinate range of the base station sensor layout , the three-dimensional coordinate range of the tag sensor layout , the number of base station sensors M, and the number of tag sensors N; Step 1.2, calculate the optimal layout coordinates of the sensors by the particle swarm optimization algorithm, including the base station sensor coordinates , and the tag sensor coordinates ; Step 1.3, calibrate the error parameters of the distance observation, including the scale factor , the zero offset , and the standard deviation .

[0008] Further, the optimal layout coordinates of the sensors are calculated by the particle swarm optimization algorithm, and the objective function formula is as follows:

[0009] Wherein, is the base station and tag sensor layout coordinates to be solved, is the matrix trace operation, is the position accuracy attenuation factor matrix.

[0010] Further, in step 1.3, the distance measurement error equation of each base station-tag sensor is as follows:

[0011] Wherein, is the distance observation, is the distance reference true value, is the scale factor error parameter, is the zero offset error parameter, is the observation noise; the distance observation and the corresponding true value distance reference true value are continuously collected by the carrier movement, the distance error is calculated, the observation whose distance error is greater than a certain multiple of the nominal ranging accuracy is eliminated according to the nominal ranging accuracy, and then the error parameters , and the standard deviation of the distance observation are solved by least squares.

[0012] Furthermore, the distance observation value after calibration in step 2 The calculation formula is as follows: .

[0013] Furthermore, in step 3, determine The number of tag sensors included is n. When n is greater than or equal to 4, the rigid body positioning algorithm is used and the marking algorithm type is 1; otherwise, the single point positioning algorithm is used and the marking algorithm type is 2.

[0014] Furthermore, in step 4, when the positioning algorithm's flag type is equal to 1, the rigid body weighted least squares method is used to solve the initial values ​​of the three-dimensional posture and position. and residuals ; When type is equal to 2, the single-point weighted least squares method is used to solve the initial value of the three-dimensional position and residuals .

[0015] Furthermore, in step 6, when type is equal to 1, the three-dimensional posture and position iteration values ​​are calculated by the rigid body positioning Gauss-Newton iteration method , ,in is the Jacobian matrix, and the initial value Update to iterated value ; When type is equal to 2, the three-dimensional position iteration value is calculated by the single-point positioning Gauss-Newton iteration method , ,in is the Jacobian matrix, and the initial value Update to iterated value .

[0016] Furthermore, in step 7, the observation weight matrix , Extract diagonal matrix operations for vectors.

[0017] The present invention also provides a single-sensor type robust high-precision three-dimensional positioning system, including: a processor and a memory, the memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute a single-sensor type robust high-precision three-dimensional positioning method as described in the above technical solution.

[0018] The advantages of the present invention are: proposing a single-sensor type robust high-precision three-dimensional positioning method, utilizing a rigid body high-precision positioning method, and based on a three-layer constraint mechanism of sensor layer geometric configuration, signal layer robust estimation and gross error elimination, and trajectory layer smoothing, to achieve high-precision robust three-dimensional positioning of a single sensor system in a complex signal transmission environment with GNSS occlusion. BRIEF DESCRIPTION OF THE DRAWINGS Figure 1 is a robust high-precision three-dimensional positioning method flowchart of a single type sensor of an embodiment of the application.

[0019] Figure 2 is a layout range of sensor parameter setting in an embodiment of the application.

[0020] Figure 3 is a result of optimal layout coordinates of a sensor in an embodiment of the application.

[0021] Figure 4 is a reference motion trajectory of a target to be positioned in an embodiment of the application.

[0022] Figure 5 is an actual motion trajectory of a target to be positioned calculated by a positioning algorithm in an embodiment of the application. DETAILED DESCRIPTION

[0023] The technical solutions of the application will be further described below with reference to the accompanying drawings.

[0024] The key of the application lies in that a plurality of tag sensors are used to form constraint of a rigid body to be positioned and rich observation information, a three-layer constraint mechanism of sensor geometry configuration, signal layer robust estimation and gross error elimination, and trajectory layer smoothing is constructed on the basis of a rigid body positioning algorithm, the constraint mechanism can guarantee that the positioning result does not appear large deviation in the case of partial observation information loss or existence of abnormality, and the three-dimensional precision and robustness of single type sensor positioning in a complex environment are improved.

[0025] The robust high-precision three-dimensional positioning method of a single sensor type provided by the application can be realized by computer software technology, and reference is made to Figure 1 , and specifically includes the following steps. Step 1, deploying a sensor and completing calibration of distance observation, including the following sub-steps. Step 1.1, setting sensor related parameters , including a three-dimensional coordinate range of base station sensor layout , a three-dimensional coordinate range of tag sensor layout , a base station sensor number M, and a tag sensor number N; Step 1.2, calculating optimal layout coordinates of a sensor by a particle swarm optimization algorithm, including base station sensor coordinates , and tag sensor coordinates ; Step 1.3, calibrating error parameters of distance observation, including a scale factor , a zero offset , and a standard deviation ; Step 2, inputting distance observation at the same time where i is the base station index, j is the tag index. The calibrated range observation is calculated according to the calibration parameters , .

[0026] Step 3, select the positioning algorithm type. Determine the number of tag sensors n contained, when n is greater than or equal to 4, adopt the rigid body positioning algorithm, mark the algorithm type type as 1; otherwise, adopt the single point positioning algorithm, mark the algorithm type type as 2.

[0027] Step 4, solve the initial value of positioning. Set the iteration number equal to 0. When type is equal to 1, adopt the rigid body weighted least square to solve the initial value of three-dimensional attitude and position and residual . When type is equal to 2, adopt the single point weighted least square method to solve the initial value of three-dimensional position and residual; Step 5, judge whether it is necessary to continue iteration: when the iteration number is greater than the threshold value or the residual is less than the threshold value , stop iteration, execute step 9. Otherwise, execute step 6; Step 6, according to the algorithm type type set in step 3, when type is equal to 1, calculate the iteration value of three-dimensional attitude and position and residual , where is the Jacobian matrix, and the initial value is updated to the iteration value . When type is equal to 2, calculate the iteration value of three-dimensional position and residual , where is the Jacobian matrix, and the initial value is updated to the iteration value . The iteration number is added by 1; Step 7, update the observation weight matrix used in step 6 iteration value calculation by using the robust M estimation of residual absolute value and minimum criterion; wherein, , is the vector extraction diagonal matrix operation; Step 8, remove the observation containing gross error by the gross error removal method of w-test hypothesis test , and then execute step 5; Step 9, the smoothed result of the previous time is obtained by Kalman filter backward smoothing algorithm The output result when type equals 1 The output result when type equals 2 .

[0028] Further, the setting of the sensor parameters in step 1.1 is as follows: Setting sensor related parameters , including the base station sensor layout three-dimensional coordinate range , the tag sensor layout three-dimensional coordinate range , the number of base station sensors M, and the number of tag sensors N. The base station sensor layout three-dimensional coordinate range The value is the maximum range that can be laid out in the x, y, and z directions , The lower limit is [0, 0, 0] and the upper limit is . The tag sensor layout three-dimensional coordinate range The value is the maximum range that can be laid out in the x, y, and z directions on the carrier to be positioned , The lower limit is , and the upper limit is . The number of base station sensors M is greater than or equal to 4; the number of tag sensors N is greater than or equal to 4; Further, the specific implementation of step 1.2 is as follows: the sensor optimal layout coordinates are calculated by a particle swarm optimization algorithm. The objective function formula is as follows:

[0029] Wherein, is the base station and tag sensor layout coordinates to be solved, is the matrix trace operation, is the position accuracy attenuation factor matrix.

[0030] Further, the specific implementation of the distance observation error parameter calibration in step 1.3 is as follows: the distance measurement error equation of each base station-tag sensor is as follows:

[0031] Wherein, is the distance observation, is the distance reference true value, is the proportional factor error parameter, is the zero offset error parameter, For observation noise. The distance observation and the corresponding true value distance reference true value are continuously collected by the carrier movement, and the distance error is calculated. According to the nominal ranging accuracy of the sensor, the observation whose distance error is greater than 3 times the nominal ranging accuracy is eliminated, and then the error parameters are solved by least square 、 And the standard deviation of the distance observation .

[0032] The embodiment takes UWB sensor positioning as an example to specifically describe the process of the application, as follows: Step 1, complete the deployment of the sensor and the calibration of the observation, including the following sub-steps: Step 1.1, set the sensor related parameters , including the three-dimensional coordinate range of the base station sensor layout , the three-dimensional coordinate range of the tag sensor layout , reference Figure 2 . The number of base station sensors M=6, and the number of tag sensors N=4; Step 1.2, calculate the optimal layout coordinates of the sensor by the particle swarm optimization algorithm. Given the number of particles is 200, and the number of iterations is 200, the particle number and iteration number two parameters and the parameters in step 1.1 are brought into the matlab particle swarm optimization function particleswarm to calculate, and the base station sensor coordinates , the tag sensor coordinates , the sensor layout diagram solved is referred to Figure 3 .

[0033] The coordinates are: ; The coordinates are: ; Step 1.3, set a reference motion trajectory, the real example is a square trajectory, the z direction value of the trajectory is kept as 1, the starting point and the ending point are [1, 1], and the trajectory passes through [1, 9], [9, 9] and [9, 1] in order, reference Figure 4 . The point sequence point[] is generated at intervals of 0.02, and the attitude is kept unchanged. The scale factor of each distance measurement value is set as , the zero offset is , the standard deviation is , the number of random distance gross errors is 0 to 5, and the size of the random distance gross error is 0 to 3m. According to the parameters, the M*N distance measurement values corresponding to each point in the point sequence point[] are generated by matlab Wherein k represents the serial number of the point, i represents the base station serial number, and j represents the tag serial number. According to the distance measurement error equation formula, the estimated value of the ranging calibration parameter obtained by least square solution is as follows: , , ; Step 2, input the distance observation of the first point [1, 1, 1] at the same time That is , wherein i is the base station serial number, j is the tag serial number, in the embodiment of the application, 6 base stations and 4 tag sensors, and finally 24 data of The vector is: ; According to the calibration of the distance observation value calculated by the calibration parameter , The vector is: ; Step 3, the distance observation of each positioning point in the embodiment contains 4 tag sensors, and a rigid body positioning algorithm is adopted, and the algorithm type type is 1; Step 4, set the iteration number to 0. Since the algorithm type type is 1, the rigid body weighted least square solution is used to calculate the initial value of positioning , : ; : ; Step 5, judge whether iteration is needed: the current iteration number is 0 less than the threshold value 5, the first iteration residual is set to 1000, and when it is greater than the threshold value 0.001, iteration is continued, and step 6 is executed.

[0034] Step 6, according to the algorithm type type set in step 3, the positioning iteration value is calculated by the rigid body positioning Gauss Newton iteration method , : ; : ; The iteration number is added by 1, and is equal to 1; Step 7, the robust M estimation of the minimum criterion of the absolute value of the residual is used to update the observation weight matrix W, and W is: ; wherein, is a vector extraction diagonal matrix operation; Step 8, set the false rejection rate of w-test hypothesis test to 0.05, and the gross error rejection method passing the w-test hypothesis test does not detect the observation containing gross error , execute step 5; Step 5 stops iteration finally, is: ; is: ; Step 9, set the smoothing window size of the Kalman filter backward smoothing algorithm to 0, and obtain the smoothing result of the previous time . The current type is equal to 1, and the output result ; The positioning result solving steps of other points in the above process motion trajectory are similar and will not be repeated. The finally generated motion trajectory actual positioning result is referred to Figure 5 , the three-dimensional position accuracy is 0.12m, which is improved by 86% compared with the rigid body positioning Gauss-Newton iteration algorithm; the 95% three-dimensional position error is 0.17m, which is improved by 91% compared with the rigid body positioning Gauss-Newton iteration algorithm; the three-dimensional attitude accuracy is 1.68°, which is improved by 14.4% compared with the rigid body positioning Gauss-Newton iteration algorithm.

[0035] On the other hand, the embodiment of the present application also provides a single sensor type robust high-precision three-dimensional positioning system, comprising: a processor and a memory, the memory is used for storing program instructions, and the processor is used for calling the storage instructions in the memory to execute the single sensor type robust high-precision three-dimensional positioning method as described in the above technical solution.

[0036] Although the UWB sensor used in the present application example performs data acquisition and positioning, other sensors are also applicable. The specific embodiments described in this paper are only illustrative of the spirit of the present application. Those skilled in the art of the present application can make various modifications or supplements to the described specific embodiments or use similar ways to replace them, but will not deviate from the spirit of the present application or exceed the scope defined by the appended claims.

Claims

1. A robust high-precision three-dimensional positioning method of the single-sensor type, characterized in that, The method comprises the following steps: Step 1, deploying sensors and completing calibration of distance observation; Step 2, input distance observation at the same time where i is the base station number, j is the tag number; the calibrated distance observation value is calculated according to the calibration parameters ; Step 3, selecting different positioning algorithms according to the number of tag sensors and marking the positioning algorithms; Step 4, set the iteration number equal to 0, according to the positioning algorithm mark the selection of different weighted least squares solution three-dimensional attitude and position initial value and residual; Step 5, determining whether to continue iteration: when the iteration number is greater than a threshold or the residual is less than a threshold , stop iteration and perform Step 9; Otherwise, step 6 is performed; Step 6, according to the algorithm type set in step 3, the three-dimensional pose and position iteration values are calculated by different iteration methods, and the iteration times are add 1; Step 7, robust M-estimation update of observation weight matrix using residual absolute value and minimum criteria for step 6 iteration value calculation; Step 8, the gross error rejection method by w-test hypothesis test to reject the observation containing gross error Step 5 is then performed. Step 9, obtaining the positioning result of the previous time through a Kalman filter backward smoothing algorithm.

2. A single sensor type robust high precision 3D positioning method as claimed in claim 1, characterized in that: The specific implementation of step 1 is as follows: Step 1.1, set sensor related parameters , including base station sensor layout three-dimensional coordinate range , tag sensor layout three-dimensional coordinate range , base station sensor number M, tag sensor number N; Step 1.2, calculate the optimal sensor layout coordinates by particle swarm optimization algorithm, including base station sensor coordinates , tag sensor coordinates ; Step 1.3, error parameters for the distance observation, including scale factor , zero offset , standard deviation .

3. A single sensor type robust high precision 3D positioning method as claimed in claim 2, characterized in that: The particle swarm optimization algorithm is used to calculate the optimal deployment coordinates of the sensors, and the objective function formula is as follows: wherein, is a matrix of position accuracy decay factors, is a matrix trace operation, is a matrix of position accuracy decay factors.

4. A single sensor type robust high precision 3D positioning method as claimed in claim 2, characterized in that: In step 1.3, the distance measurement error equation of each base station-tag sensor is as follows: wherein, is the distance observation, is the distance reference true value, is the scale factor error parameter, is the zero offset error parameter, is the observation noise; the distance observation and the corresponding true value distance reference true value are continuously collected by carrier movement, the distance error is calculated, the observation with the distance error greater than a certain multiple of the nominal ranging accuracy is removed according to the sensor nominal ranging accuracy, and then the error parameters are solved by least square , and the standard deviation of the distance observation .

5. A single sensor type robust high precision 3D positioning method as claimed in claim 1, characterized in that: The distance observation value calibrated in step 2 The calculation formula is as follows: 。 6. A single sensor type robust high precision 3D positioning method as claimed in claim 1, characterized in that: In step 3, it is judged The number n of included tag sensors, when n is greater than or equal to 4, a rigid body positioning algorithm is adopted, and the algorithm type type is marked as 1; otherwise, a single point positioning algorithm is adopted, and the algorithm type type is marked as 2.

7. A single sensor type robust high precision three-dimensional positioning method as claimed in claim 6, characterized in that: In step 4, when the flag type of the positioning algorithm is equal to 1, the initial values of the three-dimensional pose and position are solved by using the rigid body weighted least square method and the residual ; when type is equal to 2, the initial value of the three-dimensional position is solved by using the single point weighted least square method and the residual .

8. A single sensor type robust high precision 3D positioning method as claimed in claim 7, characterized in that: In step 6, when type equals 1, the three-dimensional attitude and position iteration values are calculated by the rigid body positioning Gauss-Newton iteration method , wherein is the Jacobian matrix, and the initial value is updated to the iteration value ; when type equals 2, the three-dimensional position iteration value is calculated by the single point positioning Gauss-Newton iteration method , wherein is the Jacobian matrix, and the initial value is updated to the iteration value .

9. A single sensor type robust high precision 3D positioning method as claimed in claim 1, characterized in that: In step 7, the observation weight matrix is observed , is the diagonal matrix operation for the vector 10. A robust high-precision three-dimensional positioning system of the single-sensor type, characterized in that: A processor and a memory, the memory is used for storing program instructions, and the processor is used for calling the stored instructions in the memory to execute a single-sensor type robust high-precision three-dimensional positioning method as claimed in any one of claims 1-9.

Images