CN109579832B - Personnel height autonomous positioning algorithm - Google Patents

Abstract

The invention claims to protect a highly autonomous positioning algorithm for personnel, which includes: 1. Detecting the peak characteristics of the X and Z axis accelerometers, and judging the personnel's going up and down stairs or walking status; The zero position capture and gyroscope three-dimensional dynamic fusion algorithm calculates the step height when going up and down stairs, and then calculates the height; 3. Detects the turning point in the process of going up and down the stairs through the attitude angle, and corrects the calculated height to half a floor at the turning point Integer multiples of high altitudes reduce altitude errors; this altitude algorithm does not rely on barometers and other auxiliary equipment, has high autonomy and is not easily affected by the external environment, and is suitable for various fields with complex indoor environments.

Description

A Highly Autonomous Positioning Algorithm for Personnel

technical field

The invention belongs to a height algorithm, which does not rely on barometers and any auxiliary equipment, but completely depends on inertial sensors, has good autonomy, high reliability, and is not easily affected by environmental changes. It is especially suitable for indoor environments with complex environmental changes, such as personnel positioning in nuclear power plants and fire rescue.

Background technique

With the increasing demand for applications such as situational awareness and environmental intelligence, applications based on indoor location-based services are becoming more and more popular, such as fire rescue and personnel positioning in nuclear power plants. Since the satellite signal cannot be received or the satellite signal is weak in indoor buildings, GPS altimetry cannot be used indoors.

Indoor height positioning mainly refers to accurately locating personnel to the floor where they are located. Among the current indoor personnel height positioning technologies, the most widely used is the height positioning technology based on barometers. Based on barometers and auxiliary equipment (RFID, WLAN, UWB etc.) fusion height positioning technology. The altitude positioning technology based on the fusion of barometer and auxiliary equipment needs to install equipment in indoor buildings in advance, which is only suitable for certain specific environments. Although it has high accuracy, it has poor adaptability. Although the altitude positioning technology based on the barometer has good autonomy, the barometer is easily affected by air factors such as wind speed, temperature, and humidity. In some environments with complex environmental changes, the altitude calculated by the barometer has a large Error, the error is as high as a few meters or even more than ten meters, and the personnel cannot be located on the exact floor.

In the inertial positioning system, due to the limitations of the barometer, in some indoor environments where the environmental changes are relatively complex, the altitude calculated based on the barometer is not reliable. At present, there are relatively few algorithms that only rely on inertial sensors for altitude estimation, such as the altitude algorithm based on the double integration of the accelerometer in the vertical direction. This algorithm requires high accuracy of the accelerometer and is costly. At present, the indoor height algorithm has poor autonomy and is easily affected by the environment.

Based on the above, the present invention proposes an autonomous indoor altitude algorithm based on pure inertial navigation, which only relies on MEMS accelerometers and gyroscopes, has low cost and high reliability, and can be widely used in environments with complex environmental changes. Civilian fields, such as fire rescue and personnel positioning in nuclear power plants.

Contents of the invention

The present invention aims to solve the above problems of the prior art. An indoor height positioning algorithm with good autonomy, high reliability and high precision is proposed. Technical scheme of the present invention is as follows:

A highly autonomous positioning algorithm for personnel, comprising the following steps:

1) Perform data processing on the MEMS accelerometer and MEMS gyroscope of the inertial system, mainly including: installation error, zero bias, scale factor and other error calibration, temperature compensation, and low-pass filtering. And the inertial system is installed on the waist of the pedestrian to obtain the motion acceleration of the pedestrian.

2) Perform peak detection on the X and Z axis accelerometers, which are expressed as Ax _max and Az _min respectively. Because the body swings when the person walks, two peaks appear in the accelerometer in one cycle. This patent sets a peak detection interval time _Tc , that is, after the first peak value is detected, the next peak detection is performed after the interval time _Tc , and the interference of the accelerometer secondary peak is filtered out by this time threshold method . According to the peak characteristics of the X and Z axis accelerometers, the status of people going up and down stairs or walking horizontally is judged.

3) Detect the gait of the person, calculate the step height when going up and down the stairs through the instantaneous zero position capture of the accelerometer and the three-dimensional dynamic fusion algorithm of the gyroscope, and then calculate the height;

4) The turning point in the process of going up and down the stairs is detected through the attitude angle, and the calculated height is corrected to an integer multiple of half a floor height at the turning point to obtain the height of the personnel position.

Further, the step 1) performs data processing steps including error calibration, temperature compensation, and low-pass filtering on the inertial system, specifically including:

101. Select an inertial system integrating three-axis MEMS accelerometer and gyroscope;

102. Select a rotation control system with two degrees of freedom and above;

103. Carry out the zeroing operation on the main shaft and the pitch axis of the rotation control system in step 102;

104. Place the inertial system vertically on the rotation control system, and calibrate the accelerometer and gyroscope. The calibration parameters include: scale factor error, zero offset;

105. Select an incubator, and perform temperature compensation on the data calibrated and processed in step 104;

106. Perform low-pass filtering on the data after temperature compensation in step 105.

Further, the step 1) installs the inertial system on the person under test to obtain the motion acceleration of the pedestrian, specifically including:

201. The personnel places the inertial system on the waist, collects accelerometer data after error calibration, temperature compensation, and low-pass filtering, and the personnel stand still for 1 second, collects the local gravitational acceleration components of the X, Y, and Z-axis accelerometers, and calculates the average value , recorded as g_x, g_y, g_z;

202. Subtract the mean acceleration of gravity in the static state from the X and Z-axis accelerometer data during the walking process of the person, and obtain the motion acceleration data of the person as Ax and Az respectively, and use this motion acceleration data as a research reference.

Further, the step 2) detects X, the peak characteristic of the Z-axis accelerometer, and according to the peak characteristic of the accelerometer, the state of going up and down stairs or level walking of the personnel is judged, specifically including:

301. Perform peak detection on the acceleration data, the peak value of the X-axis accelerometer is recorded as Ax _max , and the valley value of the Z-axis accelerometer is recorded as Az _min ;

302. The person walks horizontally for 2 seconds, and performs peak detection on the data of the X-axis and Z-axis accelerometers to obtain the peak-average values of the X-axis and Z-axis accelerometers, which are recorded as av_x and av_z respectively;

303. By setting a time threshold _Tc , when the first peak value is detected, the next peak detection is performed after an interval of time _Tc ;

304. Set the judgment thresholds for going upstairs and downstairs, which are respectively recorded as D ₁ , D ₂ , and D ₃ ;

305. The conditions for judging people going up and down stairs and walking horizontally are shown in Formula 1:

Further, the step 3) detects the gait of the person, calculates the step height when going up and down the stairs through the instantaneous zero position capture of the accelerometer and the three-dimensional dynamic fusion algorithm of the gyroscope, and then performs height calculation, specifically including:

401. Capture the instantaneous zero position of the accelerometer, read the values of the three-axis accelerometer, record them as Acc_x, Acc_y, and Acc_z, and obtain the modulus value Acc_norm, as shown in the following formula:

402. Perform peak detection on Acc_norm, record the peak value as Acc_min, and record the valley value as Acc_max;

403. Calculate the personnel step length DS_L, as shown in the following formula:

404. Obtain the 3D attitude angle of the person according to the 3D dynamic fusion algorithm of the gyroscope, read the heading angle and pitch angle of the i-th step, and record it as yaw _i , pitch _i ; obtain the height value of each step according to the step length, heading angle, and pitch angle h ₀ , specifically as shown in formula (4):

h ₀ ＝DS_L*sin(yaw _i )*tan(pitch _i ) (4)

405. Perform gait detection on personnel;

406. After steps 404 and 405 are completed, detect the status of the person going up and down the stairs. If the person is in the state of going upstairs, the height will be accumulated; Formula (5) shows:

Among them, H ₂ is the height value calculated at the current moment, and H ₁ is the height value of the previous step.

Further, the step 4) detects the turning point in the process of going up and down the stairs through the attitude angle, which specifically includes: when the person goes up and down the stairs, a turn will be made in the stairwell, and this curvature is usually completed in 4 to 5 steps. value to determine the turning point between floors, as shown in formula (6):

Among them, yaw _i is the heading angle of the i-th step, i=1, 2, 3, 4, 5; α is the set turning judgment threshold, and the value of the threshold is 100°.

Further, the step 4) corrects the calculated height to an integer multiple of half a floor height at the turning point, and the correction algorithm is shown in formula (7):

k=-3, -2, -1, 0, 1, 2, 3... and other integers. H ₂ is the calculated height value at the current moment, β is the height correction error threshold, and h _c is the half-floor height of the building.

Advantage of the present invention and beneficial effect are as follows:

The present invention can effectively identify the status of people going up and down the stairs in real time, and can calculate the height in real time to locate the people on the correct floor. In the process, the following beneficial effects can be achieved:

(1) Good autonomy: The algorithm uses the accelerometer and gyroscope of the inertial unit to identify and calculate the height of upstairs and downstairs, without relying on any auxiliary equipment, and has good autonomy.

(2) High reliability: the algorithm does not use a barometer, is not easily affected by the external environment, and overcomes the drawbacks of the barometer being easily affected by the environment, and is especially suitable for some application fields with complex environmental changes, such as fire rescue and nuclear power plant positioning .

(3) High precision: The algorithm adds a special correction algorithm, and the height is corrected at each floor corner, which can effectively reduce the accumulation of height errors and accurately locate personnel to the correct floor.

Description of drawings

Fig. 1 is that the present invention provides preferred embodiment algorithm flow chart

Figure 2 is a diagram of the wearing method of the inertial unit

Figure 3 is the interference of the sub-peak of the X-axis accelerometer on the results of going up and down stairs

Figure 4 is to filter out the sub-peak interference after setting the time threshold

Figure 5 is a diagram of the judgment results of the X and Z axis accelerometers going up and down stairs

Figure 6 is the indoor structure diagram of the test scene

Figure 7 is the result of height calculation

Figure 8 is the altitude calculation diagram for changing environmental factors

detailed description

The technical solutions in the embodiments of the present invention will be described clearly and in detail below with reference to the drawings in the embodiments of the present invention. The described embodiments are only some of the embodiments of the invention.

The technical scheme that the present invention solves the problems of the technologies described above is:

The present invention discloses a highly autonomous positioning algorithm for personnel only relying on inertial sensors. The flow chart of the technical solution is shown in Figure 1, specifically as follows:

First, first perform data processing on the accelerometer and gyroscope of the inertial unit:

1. Select an inertial system that integrates a three-axis MEMS accelerometer and gyroscope.

2. Select the double-circle electric turntable of the rotation control system, and perform zero-return operation on the main shaft and pitch axis of the electric turntable to make the turntable level.

3. Place the inertial system vertically on the state, and calibrate the accelerometer and gyroscope. The calibration parameters include: scale factor error, zero offset.

4. Select a high-temperature chamber, place the inertial unit in the chamber, set the temperature at -40°C to 80°C, collect the experimental data of the accelerometer, and perform temperature compensation on the accelerometer.

5. Perform low-pass filtering on the data after calibration and temperature compensation.

Second, secondly, identify the status of the person going up and down the stairs:

1. The person places the inertial system at the waist, as shown in Figure 2. Acquire calibrated, compensated, filtered accelerometer data. The person stands still for 1 second, collects the local gravitational acceleration components of the X, Y, and Z-axis accelerometers, and obtains the average value, which is recorded as g_x, g_y, and g_z.

2. Subtract the average acceleration of gravity in the stationary state from the X and Z axis accelerometer data during the walking process of the person to obtain the motion acceleration data of the person as Ax and Az respectively, and use this motion acceleration data as a research reference.

3. Perform peak detection on the accelerometer data. The peak value of the X-axis accelerometer is denoted as Ax _max , and the valley value of the Z-axis accelerometer is denoted as Az _min .

4. The person walks horizontally for 2 seconds, and collects the peak values of the X and Z axis accelerometers, which are recorded as av_x and av_z respectively.

5. Because the body of the person will sway during walking, the X-axis accelerometer will have two peaks in one cycle, that is, the main peak is followed by a false peak. This false peak will cause misjudgment of going up and down the stairs. As shown in Figure 3, during the horizontal walking process, due to the interference of the secondary peak, the horizontal walking is misjudged as going downstairs. This algorithm filters out the interference of the secondary peak by setting a time threshold _Tc , that is, after the first peak is detected, the next peak detection is performed after an interval of _Tc . This time interval method can significantly filter out the interference of secondary peaks and improve the accuracy of the judgment of going up and down stairs, as shown in Figure 4.

6. Set the judgment thresholds for going up and down the stairs, denoted as D ₁ , D ₂ , and D ₃ respectively.

7. The conditions for judging people going up and down stairs indoors and walking horizontally are shown in formula 1:

The judgment result of going up and down stairs is shown in Figure 5. When going upstairs, the peak value of the X-axis accelerometer is larger than the average peak value when walking horizontally; when going downstairs, the peak value of the X-axis accelerometer is smaller than the average peak value when walking horizontally , the trough value of the Z-axis accelerometer is smaller than the mean value of the trough value when walking horizontally. The peak characteristics of the X and Z-axis accelerometers can effectively identify the status of people going up and down stairs and horizontal walking, and the accuracy of going up and down stairs is relatively reliable.

Third, perform height calculation and height correction when people are walking:

1. Capture the instantaneous zero position of the accelerometer and read the three-axis accelerometer values at this time, which are recorded as Acc_x, Acc_y, and Acc_z respectively, and obtain the modulus value Acc_norm, as shown in the following formula:

2. Perform peak detection on Acc_norm, the peak value is recorded as Acc_min, and the valley value is recorded as Acc_max. Calculate the personnel step length DS_L, as shown in formula 3:

3. Calculate the 3D attitude angle of the person according to the 3D dynamic fusion algorithm of the gyroscope, read the heading angle and pitch angle of the i-th step, and record it as yaw _i , pitch _i ; obtain the height value of each step according to the step length, heading angle, and pitch angle h ₀ , specifically as shown in formula 4:

h ₀ ＝DS_L*cos(yaw _i -yaw _i-1 )*tan(pitch _i -pitch _i-1 ) (4)

4. Perform gait detection on personnel.

5. Identify the status of people going up and down the stairs. If they are going upstairs, the height will be accumulated; if they are going downstairs, the height will be accumulated; if they are walking horizontally, the height will not change.

Among them, H ₂ is the height value of the next step, and H ₁ is the height value of the previous step.

6. Usually, when people go up and down the stairs, they will make a turn in the stairwell, and this curvature can usually be completed in 4 to 5 steps. Therefore, the turning point between floors can be judged by using the heading angle difference, as shown in Equation 3:

Among them, yaw _i is the heading angle of the i-th step, i=1, 2, 3, 4, 5; α is the set turning judgment threshold, and the value of the threshold is 100°. In order to avoid repeated detection of the same turning point, After the algorithm detects a turning point, it detects the curvature 4 seconds later. As shown in Figure 7 and Figure 8, in the process of going up and down stairs, 98% of the turning points are detected.

5. In a conventional square building, the stairs are generally in the shape of a "zigzag", and 1 or 2 turning points will be detected during the process of going up and down the first floor. This turning point is generally at the height of half a floor or the entire floor place. When the turning point is detected, the current height value will be corrected. At the same time, when the person is detected to be walking horizontally, the height will also be corrected. The experimental group investigated nearby teaching buildings, office buildings and other buildings. The height of each floor is about 4 meters, so the corrected height _hc = 2 meters. Set the error threshold of height correction β=1.3 meters, and the specific correction algorithm is shown in formula 4:

k=-3, -2, -1, 0, 1, 2, 3... and other integers. The results of the calculation of the height of the upper and lower floors are shown in Figure 7. Figure 7 shows the process of people going from the outdoors to the teaching building and then to the outdoors (with a breeze), from the first floor to the second floor. As can be seen from Figure 7, The barometer is easily affected by the wind speed, and the height value has a large error. However, the height value calculated by the algorithm in this paper has been corrected at the corner of each floor. The correction algorithm can effectively compensate for the misjudgment of going up and down the stairs. The resulting height error enables people to be correctly positioned on the floor.

6. This paper also made a group of comparative experiments, that is, the location is chosen as the first teaching building of the school, the height of each floor is 4 meters, people walk freely, and enter an air-conditioned (refrigerated) classroom on the third floor to simulate the reduction of the air environment. , When going up from the third floor to the fourth floor, use a hot water bottle and place it next to the inertial unit to simulate the rise of the air environment. The specific results are shown in Figure 8. It can be seen that due to the change of the environment, the data of the barometer has a large error, and the change of the external environment has little influence on the algorithm of this paper.

The above embodiments should be understood as only for illustrating the present invention but not for limiting the protection scope of the present invention. After reading the contents of the present invention, skilled persons can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims (3)

1. A highly autonomous positioning algorithm for personnel is characterized in that only relying on MEMS accelerometers and gyroscopes to carry out positioning calculations, comprising the following steps: 1) Perform data processing on the MEMS accelerometer and MEMS gyroscope of the inertial system, including: installation error, zero bias, scale factor error calibration, temperature compensation, and low-pass filtering; and install the inertial system on the waist of the pedestrian to obtain pedestrian the motion acceleration; 2) Perform peak detection on the X and Z-axis accelerometers, which are respectively expressed as Ax _max and Az _min . Due to the body swing when people walk, the accelerometer has two peaks in one cycle. By setting a peak detection interval Time T _c , that is, when the first peak value is detected, the next peak detection will be performed after an interval of time T _c , through this time threshold method to filter out the interference of the accelerometer secondary peak, according to the X, Z axis accelerometer Peak characteristics, to determine the status of people going up and down stairs or walking horizontally; 3) Detect the gait of the person, calculate the step height when going up and down the stairs through the instantaneous zero position capture of the accelerometer and the three-dimensional dynamic fusion algorithm of the gyroscope, and then calculate the height; 4) The turning point in the process of going up and down the stairs is detected through the attitude angle, and the calculated height is corrected to an integer multiple of half a floor height at the turning point to obtain the height of the personnel position; The step 3) detects the gait of the personnel, calculates the step height when going up and down the stairs through the instantaneous zero position capture of the accelerometer and the three-dimensional dynamic fusion algorithm of the gyroscope, and then performs height calculation, specifically including: 501. Capture the instantaneous zero position of the accelerometer, collect the local gravitational acceleration components of the X, Y, and Z-axis accelerometers, and calculate the average value, which is recorded as g_x, g_y, g_z, and read the three-axis accelerometer values, which are respectively recorded as Acc_x, Acc_y, Acc_z, and calculate the modulus Acc_norm, as shown in the following formula:

Collect the local gravitational acceleration components of the X, Y, and Z-axis accelerometers, and calculate the average value, which is recorded as g_x, g_y, g_z 502. Perform peak detection on Acc_norm, record the valley value as Acc_min, and record the peak value as Acc_max; 503. Calculate the personnel step length DS_L, as shown in the following formula:

504. Obtain the 3D attitude angle of the personnel according to the 3D dynamic fusion algorithm of the gyroscope, read the heading angle and pitch angle of the i-th step, and record it as yaw _i , pitch _i ; obtain the height value of each step according to the step length, heading angle, and pitch angle h ₀ , specifically as shown in formula (4): h ₀ ＝DS_L*sin(yaw _i )*tan(pitch _i ) (4) 505. Perform gait detection on personnel; 506. After steps 504 and 505 are completed, detect the status of the person going up and down the stairs. If the person is in the state of going upstairs, the height will be accumulated; Formula (5) shows:

Among them, H ₂ is the height value calculated at the current moment, and H ₁ is the height value calculated in the previous step; The step 4) detects the turning point in the process of going up and down the stairs through the attitude angle, which specifically includes: when the person goes up and down the stairs, a turn will be made in the stairwell, and this curvature is usually completed in 4 to 5 steps. The turning point between floors is specifically shown in formula (6):

Among them, yaw _i is the heading angle of the i-th step, i=1, 2, 3, 4, 5; α is the set turning judgment threshold, and the value of the threshold is 100°; The step 4) corrects the calculated height to an integer multiple of half a floor height at the turning point, and the correction algorithm is shown in formula (7):

k=-3, -2, -1, 0, 1, 2, 3..., H ₂ is the height value calculated at the current moment, β is the height correction error threshold, h _c is half a floor of the building; Described step 2) detects X, the peak characteristic of Z-axis accelerometer, according to the peak characteristic of accelerometer, the state of going up and down stairs or level walking of personnel is judged, specifically includes: 401. Perform peak detection on the acceleration data, the peak value of the X-axis accelerometer is recorded as Ax _max , and the valley value of the Z-axis accelerometer is recorded as Az _min ; 402. The person walks horizontally for 2 seconds, and performs peak detection on the data of the X-axis and Z-axis accelerometers, and obtains the peak-average values of the X-axis and Z-axis accelerometers, which are recorded as av_x and av_z respectively; 403. Because the body swings when the person walks, two peaks appear in the accelerometer in one cycle. By setting a peak detection interval time T _c , that is, after the first peak value is detected, the interval time T _c will be repeated. For the next peak detection, the interference of the accelerometer sub-peak is filtered out through this time threshold method; 404. Set the judgment thresholds for going upstairs and downstairs, which are respectively recorded as D ₁ , D ₂ , and D ₃ ; 405. The conditions for judging people going up and down stairs and walking horizontally are shown in Formula 1:

2. A kind of people highly autonomous positioning algorithm according to claim 1, is characterized in that, described step 1) carries out including error calibration, temperature compensation, low-pass filter to MEMS accelerometer and MEMS gyroscope of inertial system The data processing steps include: 201. Select an inertial system that integrates a three-axis MEMS accelerometer and a gyroscope; 202. Select a rotation control system with two degrees of freedom and above; 203. Carry out the zeroing operation on the main shaft and the pitch axis of the rotation control system in step 102; 204. Place the inertial system vertically on the rotation control system, and calibrate the accelerometer and gyroscope. The calibration parameters include: installation error, scale factor error, and zero offset; 205. Select a thermostat, and perform temperature compensation on the data after calibration in step 104; 206. Perform low-pass filtering on the data after temperature compensation in step 105. 3. A kind of personnel highly autonomous positioning algorithm according to claim 2, is characterized in that, described step 2) inertial system is installed on the person under test, obtains the motion acceleration of pedestrian, specifically comprises: 301. The personnel places the inertial system on the waist, collects the accelerometer data after error calibration, temperature compensation, and low-pass filtering, and the personnel stand still for 1 second, collects the local gravitational acceleration components of the X, Y, and Z-axis accelerometers, and calculates the average value , recorded as g_x, g_y, g_z; 302. Subtract the mean acceleration of gravity in the stationary state from the X and Z-axis accelerometer data during the walking process of the person, and obtain the motion acceleration data of the person as Ax and Az respectively, and use this motion acceleration data as a research reference.

Images