Tunnel construction personnel positioning method and device

Abstract

The application discloses a tunnel construction personnel positioning method and device, and the method comprises the following steps: obtaining an initial positioning result measured by an ultra-wideband sensor, linearizing the initial positioning result measured by the ultra-wideband sensor to obtain an ultra-wideband sensor positioning result; obtaining inertial positioning data collected by an inertial sensor, and obtaining an inertial sensor positioning result through a mechanical arrangement algorithm; correcting the inertial sensor positioning result based on the ultra-wideband sensor positioning result by adopting linearization detection and a lever arm algorithm to obtain a corrected inertial sensor positioning result; fusing the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, optimizing the fused positioning result, obtaining an optimized final positioning result, and outputting the optimized final positioning result. The tunnel construction personnel positioning method realizes high-precision positioning in an indoor and dynamically changing construction environment of a tunnel.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of computer and location services, and in particular to a tunnel construction worker positioning method and device. BACKGROUND

[0002] In the intelligent construction process, accurate position information can reduce the labor intensity of workers, improve production efficiency and ensure the safety of workers; however, there are complex environmental problems such as personnel concentration, large number of equipment, serious noise pollution and messy construction materials in the construction site, which bring challenges to the positioning of construction personnel.

[0003] In indoor positioning, the Global Navigation Satellite System (GNSS) originally suitable for outdoor positioning will have weak or unreceivable signals in indoor environments and tunnels, and various positioning methods such as WiFi, Ultra-Wide Band (UWB), positioning based on inertial units, Simultaneous Localization and Mapping (SLAM), Radio Frequency Identification (RFID) tags and acoustic positioning are mainly based on the condition that signals are not disturbed during transmission; and building materials, passing vehicles or walls may affect signal propagation in complex building environments, so the current positioning system will not be accurate when used in indoor and dynamically changing construction environments.

[0004] Therefore, the prior art still needs to be improved and developed. SUMMARY

[0005] The technical problem to be solved by the present application is that the current positioning system used in indoor dynamically changing construction environments will not be accurate due to complex environments, and the tunnel construction worker positioning method and device are provided, which can realize high-precision positioning in indoor and dynamically changing construction environments in tunnels through the tunnel construction worker positioning method.

[0006] In order to solve the above-mentioned problems of the prior art, the first aspect of the present application provides a tunnel construction worker positioning method, which comprises:

[0007] obtaining an initial positioning result measured by an ultra-wide band sensor, linearizing the initial positioning result measured by the ultra-wide band sensor to obtain an ultra-wide band sensor positioning result;

[0008] obtaining inertial positioning data collected by an inertial sensor, and obtaining an inertial sensor positioning result through a mechanical arrangement algorithm;

[0009] Based on the ultra-wideband sensor positioning result, the inertial sensor positioning result is corrected by using a linearization detection and a lever arm algorithm to obtain a corrected inertial sensor positioning result;

[0010] The ultra-wideband sensor positioning result and the corrected inertial sensor positioning result are fused to obtain a fusion positioning result, and the fusion positioning result is optimized to obtain an optimized final positioning result and output.

[0011] The initial positioning result measured by the ultra-wideband sensor is obtained, and the initial positioning result measured by the ultra-wideband sensor is linearized to obtain an ultra-wideband sensor positioning result, specifically including:

[0012] The initial positioning result measured by the ultra-wideband sensor is linearized by using an extended Kalman filter to obtain an ultra-wideband sensor positioning result.

[0013] The inertial positioning data collected by the inertial sensor is obtained, and an inertial sensor positioning result is obtained by using a mechanical arrangement algorithm, specifically including:

[0014] The inertial positioning data collected by the inertial sensor includes data collected by an accelerometer and a gyroscope in the inertial sensor;

[0015] The mechanical arrangement algorithm corrects the data collected by the accelerometer and the gyroscope, and solves a pose matrix to output an inertial sensor positioning result.

[0016] The mechanical arrangement algorithm corrects the data collected by the accelerometer and the gyroscope, and solves a pose matrix to output an inertial sensor positioning result, specifically including:

[0017] The accelerometer and the gyroscope collect data and perform error compensation, correct the gyroscope data by the earth rotation rate, and then solve a pose matrix to eliminate harmful integration of the acceleration data through the pose matrix.

[0018] The inertial sensor positioning result is corrected by using a linearization detection and a lever arm algorithm, specifically including:

[0019] The linearization detection is used to determine whether the positioning information in the ultra-wideband sensor positioning result is missing;

[0020] When it is determined that the positioning information in the ultra-wideband sensor positioning result is not missing, the inertial sensor positioning result is corrected by using a lever arm algorithm based on the ultra-wideband sensor positioning result;

[0021] When it is determined that the positioning information in the ultra-wideband sensor positioning result is missing, the gyroscope data is integrated, and the inertial sensor positioning result error is constrained by the integration result.

[0022] The lever arm algorithm specifically comprises:

[0023] Based on the ultra-wideband sensor positioning result and the inertial sensor positioning result, a position measurement model is constructed, and error feedback is obtained through the position measurement model;

[0024] The error feedback is input into an error correction Kalman filter, so as to correct the inertial sensor positioning result.

[0025] The ultra-wideband sensor positioning result and the corrected inertial sensor positioning result are fused to obtain a fused positioning result, and the fused positioning result is optimized to obtain an optimized final positioning result and output.

[0026] The ultra-wideband sensor positioning result and the corrected inertial sensor positioning result are fused by using a filter to obtain a fused positioning result.

[0027] The fused positioning result is optimized by using an RTS smoothing method to obtain an optimized final positioning result and output.

[0028] The second aspect of the embodiment of the application provides a tunnel construction personnel positioning device, which comprises:

[0029] An ultra-wideband sensor positioning acquisition module acquires an initial positioning result measured by an ultra-wideband sensor, linearizes the initial positioning result measured by the ultra-wideband sensor, and obtains an ultra-wideband sensor positioning result.

[0030] An inertial sensor positioning acquisition module acquires inertial positioning data collected by an inertial sensor, and obtains an inertial sensor positioning result by using a mechanical arrangement algorithm.

[0031] A correction module corrects an inertial sensor positioning result by using linearization detection and a lever arm algorithm based on an ultra-wideband sensor positioning result, and obtains a corrected inertial sensor positioning result.

[0032] A fusion and optimization module fuses the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, and optimizes the fused positioning result to obtain an optimized final positioning result and output.

[0033] The third aspect of the embodiment of the application provides a computer readable storage medium, which stores one or more programs, and the one or more programs can be executed by one or more processors to implement steps in the tunnel construction personnel positioning method.

[0034] The fourth aspect of the embodiments of the present application provides a terminal device, comprising a processor, a memory and a communication bus; the memory stores a computer readable program which can be executed by the processor;

[0035] The communication bus realizes the connection and communication between the processor and the memory.

[0036] The processor realizes the steps in the tunnel construction personnel positioning method according to any of the above when executing the computer readable program.

[0037] Advantages: compared with the prior art, the present application provides a tunnel construction personnel positioning method and device, the method comprises the following steps: obtaining an initial positioning result measured by an ultra-wideband sensor, linearizing the initial positioning result measured by the ultra-wideband sensor to obtain an ultra-wideband sensor positioning result; obtaining inertial positioning data collected by an inertial sensor, and obtaining an inertial sensor positioning result through a mechanical arrangement algorithm; based on the ultra-wideband sensor positioning result, correcting the inertial sensor positioning result by using linear detection and a lever arm algorithm to obtain a corrected inertial sensor positioning result; fusing the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, and optimizing the fused positioning result to obtain an optimized final positioning result and output. Through the above method, the positioning data of the corresponding ultra-wideband sensor and inertial sensor is obtained, the corresponding inertial sensor positioning result is corrected by using linear detection and a lever arm algorithm, the corrected inertial sensor positioning result is obtained, and after fusion and optimization, the ultra-wideband sensor and the inertial positioning sensor are fused to realize high-precision and robust positioning; and in the process, the lever arm algorithm is used to correct the cumulative error of the inertial sensor using the ultra-wideband sensor information, so that the data obtained by the inertial sensor is more accurate; in the method, when the ultra-wideband sensor data is abnormal, the positioning result of the inertial sensor is corrected by using linear detection algorithm; and the present application finally realizes data fusion of the ultra-wideband sensor and the inertial sensor, and optimizes the positioning result by using RTS smoothing method (Rauch-tung-sriebel, RTS), and finally realizes high-precision positioning in an indoor and tunnel dynamic change construction environment. BRIEF DESCRIPTION OF DRAWINGS

[0038] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0039] Figure 1A flow chart of a tunnel construction personnel positioning method provided for an embodiment of the present application is shown in FIG. 1.

[0040] Figure 2 A positioning frame schematic diagram provided for an embodiment of the present application is shown in FIG. 2.

[0041] Figure 3 A three-edge positioning model schematic diagram provided for an embodiment of the present application is shown in FIG. 3.

[0042] Figure 4 A strapdown inertial navigation principle block diagram provided for an embodiment of the present application is shown in FIG. 4.

[0043] Figure 5 A relative posture relationship schematic diagram between a head, a heel and a waist in a gait cycle provided for an embodiment of the present application is shown in FIG. 5.

[0044] Figure 6 A tunnel construction personnel sensor positioning device installation schematic diagram provided for an embodiment of the present application is shown in FIG. 6.

[0045] Figure 7 A principle block diagram of a tunnel construction personnel positioning device provided for an embodiment of the present application is shown in FIG. 7. DETAILED DESCRIPTION

[0046] The present application provides a tunnel construction personnel positioning method and device, in order to make the purpose, technical solution and effect of the present application more clear and explicit, the present application is further described in detail below with reference to the drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application.

[0047] Those skilled in the art of the present technology can understand that, unless specifically stated, the singular form "a", "an" and "the" used herein also includes the plural form. It should be further understood that the use of the word "comprise" in the specification of the present application means that the features, integers, steps, operations, elements and / or components exist, but do not exclude the existence or addition of one or more other features, integers, steps, operations, elements, components and / or their combinations. It should be understood that when we say that an element is "connected" or "coupled" to another element, it can be directly connected or coupled to the other element, or there can be intermediate elements. In addition, the "connection" or "coupling" used herein can include wireless connection or wireless coupling. The phrase "and / or" used herein includes all or any single unit and all combinations of the associated listed items.

[0048] Those skilled in the art can understand that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. It should also be understood that terms such as those defined in commonly used dictionaries should be interpreted as having a meaning that is consistent with their meaning in the context of the relevant art and will not be interpreted in an idealized or overly formal sense unless specifically so defined herein.

[0049] In addition, if the description of "first", "second" and the like is involved in the embodiments of the present application, the description of "first", "second" and the like is only for the purpose of description, and cannot be understood as indicating or implying the relative importance of the indicated technical features or implicitly indicating the number of the indicated technical features. Therefore, the features limited by "first", "second" can be explicitly or implicitly included at least one of the features. In addition, the technical solutions of various embodiments can be combined with each other, but it must be based on the realization of ordinary skilled in the art, when the combination of technical solutions appears contradictory or unachievable, it should be considered that the combination of technical solutions does not exist, nor within the protection scope required by the present application.

[0050] With the application of intelligent construction method, it is particularly important to ensure the safety of construction personnel, and in the intelligent construction process, accurate position information can reduce the labor intensity of workers, improve production efficiency and ensure the safety of workers. However, there are many complex environmental problems in the construction site, such as personnel concentration, large number of equipment, serious noise pollution, and messy construction materials, which bring challenges to the positioning of construction personnel.

[0051] At present, positioning is carried out by using global navigation satellite system (GNSS), which has achieved good results in outdoor construction site personnel positioning; however, the signal of GNSS in indoor environment will be weak or unable to be received. Various sensors will be disturbed by the environment when used indoors, such as WiFi, ultra-wide band (UWB), positioning based on inertial unit, simultaneous localization and mapping (SLAM), radio frequency identification (RFID) tag and acoustic positioning; among them, UWB has the characteristics of strong penetration and high precision in weak signal interference environment; however, these positioning methods are mainly based on the condition that the signal is not disturbed in the transmission process, when used in indoor and tunnel, the building materials, passing vehicles or walls may affect the signal propagation in complex building environment, and a single UWB positioning system cannot be used in dynamically changing construction environment.

[0052] At present, multi-sensor fusion positioning has obvious advantages in indoor building positioning, and the tunnel construction personnel positioning method provided by the application fuses UWB and inertial sensors (Inertial Measurement Unit, IMU), wherein the advantage of UWB is that it effectively limits the error of inertial positioning, and at the same time, inertial positioning provides high-precision positioning in a short period to overcome UWB error, so it is feasible to fuse ultra-wideband and inertial positioning systems to achieve high precision and robustness. Based on this, the application provides a tunnel construction personnel positioning method and device, which can realize high-precision positioning in indoor and dynamically changing construction environments without relying on external GNSS signals.

[0053] Example method

[0054] As shown in Figure 1 A flow chart of a tunnel construction personnel positioning method provided by an embodiment of the application is shown, which can be applied to a terminal device. In the embodiment of the application, the method is described in combination with Figure 1 The method includes the following steps:

[0055] Step S10, obtaining an initial positioning result measured by an ultra-wideband sensor, linearizing the initial positioning result measured by the ultra-wideband sensor to obtain an ultra-wideband sensor positioning result;

[0056] In the application, the initial positioning result is measured by an ultra-wideband sensor, the corresponding initial positioning result measured by the ultra-wideband sensor is obtained, and the initial positioning result is linearized to convert the nonlinear electromagnetic wave signal in a complex construction environment into a linearized ultra-wideband sensor positioning result.

[0057] The ultra-wideband sensor (Ultra-Wide Band, UWB) adopts ultra-wideband technology, and the implementation of the positioning function can be divided into two steps of "distance measurement-positioning", that is, obtaining the node coordinate position from the reference node (usually called base station, landmark, etc.) arranged at a fixed coordinate position, and then calculating the coordinate of the to-be-measured node through positioning technology; wherein distance measurement can adopt various algorithms such as double-side two-way distance measurement and RSSI distance measurement positioning algorithm, so as to measure the distance between the base station and the to-be-measured node; wherein the positioning method in the application adopts trilateration, as shown in Figure 3As shown, the position of the known point can be effectively estimated to obtain the coordinate of the target node. In the present application, the trilateration method is first based on ToA to measure the distance of three base stations, and then the measured distance between the base station and the tag is taken as the radius, the known position of the base station is taken as the center of the circle, and three circles are drawn. The intersection or overlapping part of the three circle lines is all the possible positions of the target tag. Wherein, TOA is the propagation time of the signal emitted by the positioning tag to the base station, which is used to calculate the distance between the positioning tag and the base station, and the positioning is realized by space intersection. In the trilateration method, the traditional trisection method based on linear least square calculates three sets of ranging information without any redundant information. However, in a complex indoor environment, signals usually reach the receiver through multiple reflections. Therefore, multi-base station measurement and nonlinear geometric method are adopted to solve the problems of positioning stability and accuracy. The data redundancy in the calculation process can ensure that at least three reliable measurement values are involved in the coordinate calculation, and the nonlinear equation reduces the influence of signal reflection.

[0058] Further, the initial positioning result measured by the ultra-wideband sensor is obtained, and the initial positioning result measured by the ultra-wideband sensor is linearized to obtain the ultra-wideband sensor positioning result, specifically including:

[0059] The initial positioning result measured by the ultra-wideband sensor is linearized by using an extended Kalman filter to obtain the ultra-wideband sensor positioning result.

[0060] After obtaining the initial positioning result measured by the UWB, since the electromagnetic wave signal in the complex construction environment is generally nonlinear, the extended Kalman filter (EKF algorithm) is used to perform Taylor series expansion on the reference point of the nonlinear system, which can linearize the nonlinear system, and then perform Kalman filtering.

[0061] Further, the extended Kalman filter represents the relationship between the system model and the measurement and state by the following formula as shown in formula (1) and (2):

[0062] x k+1 =f(x k-1 ,u k ,ω k ) (1)

[0063] z k =h(X k ,v k ) (2)

[0064] Wherein, f is a control matrix, h is a measurement matrix, x is an estimated state value, z is a measurement value, u is a control parameter, ω is noise, k is a time; the estimated state value at k+1 time is solved by formula (1), and the updated measurement value is obtained according to (1) according to formula (2) based on the estimated state value solved by the above iteration, and the updated measurement value z k Corresponding to the next x k , for formula (1) to solve the state value estimation; based on the estimated state value solved by the above iteration, the best estimation of sensor positioning data is finally realized.

[0065] Step S20, the inertial positioning data collected by the inertial sensor is acquired, and the inertial sensor positioning result is obtained through the mechanical arrangement algorithm.

[0066] The inertial positioning data is acquired through the inertial sensor, and the corresponding inertial sensor positioning result is acquired through the acquired inertial positioning data. Wherein, the positioning based on the inertial sensor adopts a strap-down inertial navigation system (SINS) method to realize an inertial navigation system, a mechanical arrangement equation is a key to realize an inertial navigation algorithm, and the inertial navigation algorithm is established by deducing Newton's law of mechanics. By analyzing the relationship between IMU data and the acceleration, angular velocity and speed of the carrier in the navigation system, the mechanical arrangement equation of the strap-down inertial navigation system in the global coordinate system can be obtained; wherein the accelerometer and the gyroscope in the inertial sensor can collect acceleration and angular velocity in different directions, and the sensor data of the two has complementary characteristics, and the accurate sensor attitude data is obtained by fusing the two.

[0067] Further, the inertial positioning data collected by the inertial sensor is acquired, and the inertial sensor positioning result is obtained through the mechanical arrangement algorithm, and specifically includes:

[0068] The inertial positioning data collected by the inertial sensor includes the data collected by the accelerometer and the gyroscope in the inertial sensor.

[0069] The mechanical arrangement algorithm corrects the data collected by the accelerometer and the gyroscope, and solves the attitude matrix, and outputs the inertial sensor positioning result.

[0070] In the present application, the positioning based on the inertial sensor adopts a strap-down inertial navigation system (SINS) method to realize an inertial navigation system, and a mechanical arrangement equation is a key to realize an inertial navigation algorithm, and the inertial navigation algorithm is established by deducing Newton's law of mechanics; through analyzing the relationship between IMU (Inertial Measurement Unit, inertial sensor) data and acceleration, angular velocity and velocity of a carrier in a navigation system, a mechanical arrangement equation of the strap-down inertial navigation system in a global coordinate system can be obtained.

[0071] The mechanical arrangement algorithm corrects the data collected by the accelerometer and the gyroscope, solves a posture matrix, and outputs an inertial sensor positioning result, and specifically includes:

[0072] The accelerometer and the gyroscope collect data and perform error compensation, the gyroscope data is corrected by an earth rotation rate, and then the posture matrix is solved, and the acceleration data is eliminated by the posture matrix.

[0073] As shown in Figure 4 , it is a block diagram of a strap-down navigation principle provided by the embodiment of the present application, wherein the accelerometer and the gyroscope collect data and perform error compensation; the acceleration data is eliminated by the posture matrix. The gyroscope data is corrected by an earth rotation rate, and then the posture matrix is solved. The position, velocity and posture matrix are obtained by using a strap-down inertial positioning method.

[0074] The inertial navigation mechanical equation is as follows:

[0075] The position, velocity and acceleration of the pedestrian relative to the world coordinate system are respectively Wherein wb represents that the parameter is a parameter of the pedestrian relative to the world coordinate system; the acceleration relative to the inertial system is α ib , wherein ib represents that the parameter is a parameter of the pedestrian relative to the inertial coordinate system, i represents the inertial coordinate system, b represents the carrier, that is, the IMU coordinate system, and also the coordinate system corresponding to the IMU worn by the pedestrian, and w represents the world coordinate system, The relationship is as shown in formulas (3) and (4):

[0076]

[0077]

[0078] Wherein t represents time, the lower limit w of the integral of formula (3) and (4) represents velocity; wherein α ib And the relationship between r wb is as shown in formula (5), wherein:

[0079]

[0080] where α ib and has the following conversion relationship:

[0081]

[0082] where the lower limit of integration w represents the velocity, and ω represents the rate, and iw represents the parameter representation of the world coordinate system relative to the inertial coordinate system;

[0083] From the above formula (6), formula (7) can be obtained:

[0084]

[0085] The data directly measured by the accelerometer is called specific force (f), and the physical meaning of f is the difference between the acceleration of the carrier relative to the inertial system and the gravitational acceleration:

[0086] f = α ib -G (8)

[0087] In the formula, G is the acceleration of the carrier relative to the inertial system at a certain point on the surface of the object, including the influence of the earth's gravity and rotation.

[0088] G = g + ω iw ×(ω iw ×r wb )(9)

[0089] Substituting equations (7) and (9) into equation (8), the specific force equation is shown in formula (10):

[0090]

[0091] The present application adopts quaternion differential equation for attitude updating, assuming is a unit quaternion representing the attitude of the world coordinate system to the carrier coordinate system, representing the updated unit quaternion, then the corresponding attitude quaternion differential equation is:

[0092]

[0093] where Ω * is an attitude matrix, represents the projection of the rate of the carrier coordinate system relative to the world coordinate system in the carrier coordinate system,

[0094]

[0095] where,

[0096] is measured directly by the gyroscope and represents the projection of the rate of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system, represents the projection of the rate of the world coordinate system relative to the inertial coordinate system in the world coordinate system, represents the projection of the rate of the carrier coordinate system relative to the world coordinate system in the carrier coordinate system, represents the direction cosine matrix from the world coordinate system to the carrier coordinate system, represents the direction cosine matrix from the carrier coordinate system to the world coordinate system, and respectively represent the components of the projection of the rate of the carrier coordinate system relative to the world coordinate system in the carrier coordinate system on the x, y, z axes.

[0097] Step S30, based on the ultra-wideband sensor positioning result, the inertial sensor positioning result is corrected by using linear detection and lever arm algorithm to obtain the corrected inertial sensor positioning result.

[0098] After obtaining the inertial sensor data, the data obtained by the inertial sensor will have errors due to random noise, cross-coupling error and scale factor, therefore, in the present application, the inertial sensor positioning result is corrected by using lever arm algorithm with the aid of the ultra-wideband sensor positioning result, and when the ultra-wideband sensor positioning result has data packet loss, linear detection can also be used to correct the inertial sensor positioning result.

[0099] Further, the inertial sensor positioning result is corrected by using linear detection and lever arm algorithm, specifically including:

[0100] Using linear detection to determine whether the positioning information in the ultra-wideband sensor positioning result is missing;

[0101] When it is determined that the positioning information in the ultra-wideband sensor positioning result is not missing, the inertial sensor positioning result is corrected by using lever arm algorithm based on the ultra-wideband sensor positioning result;

[0102] When it is determined that the positioning information in the ultra-wideband sensor positioning result is missing, the gyroscope data is integrated, and the error of the inertial sensor positioning result is constrained by the integration result.

[0103] Specifically, in the present application, the wireless signal can be affected by various conditions, such as metal materials, which can cause data packet loss in the ultra-wideband sensor signal, so when the signal is corrected by the ultra-wideband sensor, the corresponding linear detection is first judged to determine whether the positioning information in the set fixed threshold interval is missing or the number of positioning information does not meet the requirements, and then it is determined that the positioning information in the positioning result of the ultra-wideband sensor is missing, that is, the UWB signal is insufficient to correct the positioning result of the inertial sensor, at which point the corresponding integral processing of the gyroscope data is performed to constrain the error of the inertial sensor positioning result; and when it is determined that the positioning information in the positioning result of the ultra-wideband sensor is not missing, the positioning result of the inertial sensor is corrected based on the positioning result of the ultra-wideband sensor using a lever arm algorithm. Wherein, the fixed threshold interval is set by the user according to the specific environment, the missing positioning information is judged by a pre-set threshold, and the number of positioning information is also judged by a pre-set threshold; the corresponding integral processing of the gyroscope data is performed to constrain the error of the inertial sensor positioning result, which specifically includes integrating the change of the gyroscope direction for several seconds, and if the angle change is less than 10°, it will be considered as a straight line to constrain the cumulative error of the inertial navigation system, wherein the several seconds of the line is a time interval set by the user in advance.

[0104] Further, the lever arm algorithm specifically includes:

[0105] Based on the positioning result of the ultra-wideband sensor and the positioning result of the inertial sensor, a position measurement model is constructed, and an error feedback is obtained through the position measurement model;

[0106] The error feedback is input into an error correction Kalman filter, so as to correct the positioning result of the inertial sensor.

[0107] Specifically, the mechanical arrangement algorithm cannot prevent the inertial sensor from accumulating a large amount of error in a short time during the positioning process, regardless of whether the device is installed on the foot, waist or head, therefore, a large number of relatively accurate measurements are needed to ensure the reliability of the inertial navigation system positioning; and the behavior semantics of the pedestrian (such as turn detection) and the motion model (such as the pedestrian dead reckoning method) can provide correction help for the mechanical arrangement INS. As shown in Figure 5 The relative posture relationship between the head, heel and waist in the gait cycle provided by the embodiment of the present application is shown in the schematic diagram, and each person repeatedly four kinds of gaits when walking, and the pedestrian repeatedly four kinds of actions Figure 5 (e) and (a) are repeated actions); in Figure 5In the middle, it is difficult to accurately determine the precise pose relationship between the head device and the feet corresponding to actions (a), (c) and (d) in the middle, however, if action (b) occurs, the action can be accurately detected, and the relative pose of the head and the feet can be accurately determined; as Figure 5 shown, action (b) is a transient standing pose that occurs during the movement process, which is very common in crowds, that is, at this time, the relative pose between the head and the heel (i.e. the lever arm) is very consistent in each gait cycle. Therefore, the ultra-wideband sensor positioning result obtained by the UWB signal on the helmet can be used to correct the cumulative error of the heel inertial sensor, and the position measurement model is shown in the following formula (12):

[0108]

[0109] wherein, and represent the positions of UWB and heel respectively, is the state transition matrix, and heel ε represents the position error of ultra-wideband positioning, and L is the relative attitude, i.e. the lever arm transmission of ultra-wideband and heel inertial navigation device, which corrects the inertial sensor positioning result through the above formula.

[0110] The establishment of the lever arm model can accurately convert the UWB positioning coordinates into foot coordinates in real time, and then apply it to the error correction Kalman filter through error feedback to correct the position, velocity and attitude of the heel MEMS inertial navigation.

[0111] In addition, as Figure 6 shown, the tunnel construction personnel sensor positioning device installation schematic diagram provided by the embodiment of the present application is shown, wherein the UWB is installed on the safety helmet, i.e. corresponding to the user's head, and the inertial sensor can be set on the user's waist or feet, wherein in a preferred embodiment, the inertial sensor is set on the user's feet to realize the lever arm algorithm.

[0112] Step S40, fusing the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, and optimizing the fused positioning result to obtain an optimized final positioning result and output.

[0113] After obtaining the corresponding ultra-wideband sensor positioning result and the corrected inertial sensor positioning result, the two positioning results can be fused, and the fused structure can be optimized to obtain the final accurate optimized final positioning result which can be output.

[0114] Further, the ultra-wideband sensor positioning result and the modified inertial sensor positioning result are fused to obtain a fusion positioning result, and the fusion positioning result is optimized to obtain an optimized final positioning result and output, specifically comprising:

[0115] The ultra-wideband sensor positioning result and the modified inertial sensor positioning result are fused by using a filter to obtain a fusion positioning result;

[0116] The fusion positioning result is optimized by using an RTS smoothing method to obtain an optimized final positioning result and output.

[0117] Specifically, after the ultra-wideband sensor positioning result and the modified inertial sensor positioning result are obtained, a filter is used for fusion, wherein the filter fusion can use original Kalman filter, unscented Kalman filter, adaptive Kalman filter and other methods for filter fusion; and the fusion positioning result after fusion is optimized by using an RTS smoothing method, which completes T times of forward recursion from initial time 0 to time T, and then repeats T times backward from time T, to complete the entire RTS smoothing process; in essence, the RTS smoothing process is a Kalman filtering process, that is, a Kalman smoothing algorithm; wherein the input and output parameters of the forward recursion and the backward recursion are opposite; the estimation accuracy and stability of the Kalman filter are significantly improved by using the RTS smoothing method, so that the output data is more accurate.

[0118] Further, the present application can Figure 2Further description is made to the high-precision external parameter correction method of the visual and laser sensors, specifically, in the UWB measurement process, the ranging error compensation model can be used to compensate the UWB ranging error before the obtained initial positioning result is linearized, so as to ensure the ranging accuracy in the indoor environment as much as possible; that is, the scheme can be described as follows: obtaining the initial positioning result measured by the UWB sensor, using the ranging error compensation model to compensate the UWB ranging error, linearizing the initial positioning result measured by the UWB sensor to obtain the UWB sensor positioning result; obtaining the inertial positioning data collected by the inertial sensor, and obtaining the inertial sensor positioning result through the mechanical arrangement algorithm; based on the UWB sensor positioning result, using the linearization detection and lever arm algorithm to correct the inertial sensor positioning result to obtain the corrected inertial sensor positioning result; fusing the UWB sensor positioning result and the corrected inertial sensor positioning result to obtain the fused positioning result, and optimizing the fused positioning result to obtain the optimized final positioning result and output. In the present application, the sensor installation position can be further adjusted, and the UWB or IMU can be installed on the waist, etc.; and in the sensor positioning, different sensor positioning data combinations can also be used, such as adding image information, sound information, other radio frequency data, etc.

[0119] As shown in Figure 7 The second aspect of the embodiment of the present application provides a tunnel construction personnel positioning device, which comprises:

[0120] A UWB sensor positioning acquisition module 71 is configured to obtain an initial positioning result measured by a UWB sensor, linearize the initial positioning result measured by the UWB sensor to obtain a UWB sensor positioning result.

[0121] An inertial sensor positioning acquisition module 72 is configured to obtain inertial positioning data collected by an inertial sensor, and obtain an inertial sensor positioning result through a mechanical arrangement algorithm.

[0122] A correction module 73 is configured to correct the inertial sensor positioning result based on the UWB sensor positioning result, using a linearization detection and lever arm algorithm to obtain a corrected inertial sensor positioning result.

[0123] A fusion and optimization module 74 is configured to fuse the UWB sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, and optimize the fused positioning result to obtain an optimized final positioning result and output.

[0124] The third aspect of the embodiments of the present application provides a computer readable storage medium, which stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps in the tunnel construction personnel positioning method.

[0125] The fourth aspect of the embodiments of the present application provides a terminal device, which comprises a processor, a memory and a communication bus; the memory stores a computer readable program which can be executed by the processor;

[0126] The communication bus realizes the connection and communication between the processor and the memory.

[0127] The processor realizes the steps in the tunnel construction personnel positioning method when executing the computer readable program.

[0128] In summary, the present application provides a tunnel construction personnel positioning method and device, which comprises obtaining an initial positioning result measured by an ultra-wideband sensor, linearizing the initial positioning result measured by the ultra-wideband sensor to obtain an ultra-wideband sensor positioning result; obtaining inertial positioning data collected by an inertial sensor, and obtaining an inertial sensor positioning result through a mechanical programming algorithm; correcting the inertial sensor positioning result based on the ultra-wideband sensor positioning result by using linearization detection and a lever arm algorithm to obtain a corrected inertial sensor positioning result; fusing the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, and optimizing the fused positioning result to obtain an optimized final positioning result and outputting the same. Through the above method, the positioning data of the corresponding ultra-wideband sensor and inertial sensor is obtained, the corresponding inertial sensor positioning result is corrected by using linearization detection and a lever arm algorithm to obtain a corrected inertial sensor positioning result, and after corresponding fusion and optimization, the ultra-wideband sensor and the inertial positioning sensor are fused to realize high-precision and robust positioning. In the process, the lever arm algorithm is used to correct the cumulative error of the inertial sensor using the ultra-wideband sensor information, so that the data obtained by the inertial sensor is more accurate. In the method, when the ultra-wideband sensor data is abnormal, the positioning result of the inertial sensor is corrected by using linear detection algorithm. Furthermore, the present application finally realizes the data fusion of the ultra-wideband sensor and the inertial sensor, and optimizes the positioning result by using RTS smoothing method (Rauch-tung-sriebel, RTS), and finally realizes high-precision positioning in an indoor and tunnel dynamic change construction environment.

[0129] It should be noted that, as used in this document, the terms "includes" and / or "containing", or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements is not necessarily limited to those elements, but can include other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0130] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer readable storage medium, and when the computer program is executed, the processes of the above-mentioned embodiments can be included. Any reference to memory, storage, database or other medium used in each embodiment of the present application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0131] It should be understood that the application is not limited to the above examples, and those skilled in the art can make improvements or changes according to the above description, and all these improvements and changes are within the scope of the appended claims of the present application.

Claims

1. A method of locating a tunneling personnel, characterized in that, The method comprises: obtaining an initial positioning result measured by an ultra-wideband sensor, linearizing the initial positioning result measured by the ultra-wideband sensor to obtain an ultra-wideband sensor positioning result; obtaining inertial positioning data collected by an inertial sensor, and obtaining an inertial sensor positioning result through a mechanical arrangement algorithm; based on the ultra-wideband sensor positioning result, correcting the inertial sensor positioning result by using linearization detection and a lever arm algorithm to obtain a corrected inertial sensor positioning result; the correction of the inertial sensor positioning result by using the linearization detection and the lever arm algorithm specifically comprises: using linearization detection to determine whether the positioning information in the ultra-wideband sensor positioning result is missing; when it is determined that the positioning information in the ultra-wideband sensor positioning result is not missing, correcting the inertial sensor positioning result based on the ultra-wideband sensor positioning result by using a lever arm algorithm; when it is determined that the positioning information in the ultra-wideband sensor positioning result is missing, integrating the gyroscope data for processing, and constraining the error of the inertial sensor positioning result through the integrated processing result; fusing the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, optimizing the fused positioning result, obtaining an optimized final positioning result, and outputting the optimized final positioning result.

2. A method of locating a tunnel construction worker according to claim 1, wherein, the obtaining of the initial positioning result measured by the ultra-wideband sensor, the linearization of the initial positioning result measured by the ultra-wideband sensor, and the obtaining of the ultra-wideband sensor positioning result specifically comprise: linearizing the initial positioning result measured by the ultra-wideband sensor by using an extended Kalman filter to obtain the ultra-wideband sensor positioning result.

3. The method of claim 1, wherein, the obtaining of the inertial positioning data collected by the inertial sensor and the obtaining of the inertial sensor positioning result through the mechanical arrangement algorithm specifically comprise: the obtaining of the inertial positioning data collected by the inertial sensor comprises the obtaining of data collected by an accelerometer and a gyroscope in the inertial sensor; the mechanical arrangement algorithm corrects the data collected by the accelerometer and the gyroscope, solves a posture matrix, and outputs the inertial sensor positioning result.

4. A method of locating a tunnel construction worker according to claim 3, wherein, the correction of the data collected by the accelerometer and the gyroscope by the mechanical arrangement algorithm, the solving of the posture matrix, and the output of the inertial sensor positioning result specifically comprise: the accelerometer and the gyroscope collect data, perform error compensation, correct the gyroscope data by using the earth rotation rate, solve the posture matrix, and eliminate harmful integration of acceleration data through the posture matrix.

5. The method of claim 1, wherein, the lever arm algorithm specifically comprises: based on the ultra-wideband sensor positioning result and the inertial sensor positioning result, a position measurement model is constructed, and an error feedback is obtained through the position measurement model; the error feedback is input into an error correction Kalman filter, so as to correct the inertial sensor positioning result.

6. The method of claim 1, wherein, the fusion of the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result, the obtaining of a fused positioning result, the optimization of the fused positioning result, the obtaining of an optimized final positioning result, and the output of the optimized final positioning result specifically comprise: The filter is used to fuse the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result. The fused positioning result is optimized by an RTS smoothing method to obtain an optimized final positioning result and output the optimized final positioning result.

7. A tunnel worker positioning device for use in the tunnel worker positioning method according to any one of claims 1 to 6, characterized in that The device comprises: An ultra-wideband sensor positioning acquisition module is configured to acquire an initial positioning result measured by an ultra-wideband sensor, linearize the initial positioning result measured by the ultra-wideband sensor, and obtain an ultra-wideband sensor positioning result. An inertial sensor positioning acquisition module is configured to acquire inertial positioning data collected by an inertial sensor, and obtain an inertial sensor positioning result by a mechanical programming algorithm. A correction module is configured to correct the inertial sensor positioning result by a linearization detection and a lever arm algorithm based on the ultra-wideband sensor positioning result, and obtain a corrected inertial sensor positioning result. A fusion and optimization module is configured to fuse the ultra-wideband sensor positioning result and the corrected inertial sensor positioning result to obtain a fused positioning result, and optimize the fused positioning result to obtain an optimized final positioning result and output the optimized final positioning result.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores one or more programs which can be executed by one or more processors to implement the steps in the tunnel construction personnel positioning method according to any one of claims 1-6.

9. A terminal device, comprising: It comprises: A processor, a memory and a communication bus; the memory stores a computer readable program which can be executed by the processor; The communication bus realizes the connection and communication between the processor and the memory; The processor executes the computer readable program to implement the steps in the tunnel construction personnel positioning method according to any one of claims 1-6.

Images