Indoor vehicle and pedestrian real-time positioning method

Abstract

The invention provides an indoor vehicle and pedestrian real-time positioning method, and relates to the surveying and mapping science and technical field, and the method comprises the steps: employing a laser radar to obtain a vector map in a test room; the head and the tail of a road in the vector map are used as nodes, and geometric data of the road and a topological relation between the roads are obtained through coordinates of the nodes; and based on the topological relation, on the basis of the known road on which the pedestrian and the vehicle walk currently, constraining the offset dead reckoning trajectory to a correct road range through road matching, and completing real-time positioning of the indoor vehicle and the pedestrian. According to the invention, real-time positioning of indoor vehicles and pedestrians can be realized only by using a mobile phone and a low-cost sensor, and the problems of relatively high hardware and installation cost, great layout planning difficulty and the like caused by conventional indoor positioning by using an external confidence source are solved.

Description

Technical Field

[0001] The present invention relates to the field of surveying and mapping science and technology, and particularly to a method for real-time positioning of indoor vehicles and pedestrians. Background Art

[0002] With the explosive growth in the number of modern buildings, their internal environments have become complex and diverse. Although the Global Navigation Satellite System (GNSS) can provide location services for most outdoor areas, it cannot be used for indoor positioning because signals cannot penetrate building exteriors. However, people spend a large amount of time indoors in their daily work and life. Once entering an unfamiliar indoor area, the need for positioning and navigation is extremely urgent. Therefore, indoor positioning technologies that do not rely on satellite navigation are extremely urgent. Currently, there are various indoor positioning technologies, but positioning technologies based on external signal sources such as Bluetooth, Wi-Fi, Ultra-Wideband (UWB), and ultrasonic waves rely too much on base stations, increasing the installation and maintenance costs of base stations. The geomagnetic matching positioning technology relies too much on the matching library and environmental factors and cannot achieve stable and reliable positioning in complex and changeable indoor environments. Vision and other matching technologies rely on a large amount of terminal computing power, resulting in high implementation costs. Therefore, it has become an urgent problem to implement an indoor positioning technology that only requires low-cost sensors (such as smartphones and OBDII devices), low computing power, no base stations, no maintenance, can meet the needs of people's daily lives, and is easy to promote in different indoor environments and among various groups of people. Summary of the Invention

[0003] In view of the above deficiencies in the prior art, the present invention provides a method for real-time positioning of indoor vehicles and pedestrians. In the case of knowing the initial position and heading angle of the carrier, it does not rely on base stations for electromagnetic wave or ultrasonic ranging, and only uses low-cost sensors such as inertial navigation, barometers, and OBDII devices to perform positioning in indoor environments with obvious structural features.

[0004] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A method for real-time positioning of indoor vehicles and pedestrians, comprising the following steps:

[0005] S1. Obtain the vector map of the test indoor area;

[0006] S2. Use the start and end points of the roads in the vector map as nodes, and obtain the geometric data of the roads and the topological relationship between the roads through the node coordinates;

[0007] S3. Based on the topological relationship, on the basis of knowing the roads currently traveled by pedestrians and vehicles, constrain the offset dead reckoning trajectory to the correct road range through road matching to complete the real-time positioning of indoor vehicles and pedestrians.

[0008] The beneficial effects of the present invention are as follows: The present invention provides a low-cost indoor vehicle and pedestrian real-time positioning method based on map and multi-sensor fusion. When the initial position and heading angle of the carrier are known, this method does not rely on base stations for electromagnetic wave or ultrasonic ranging. Only using low-cost sensors such as inertial navigation, barometer, and vehicle diagnostic system OBDII (On-Board Diagnostics II, OBDII) devices, it can perform positioning in an indoor environment with obvious structural features, that is, it can obtain observation data such as the angular velocity, acceleration, air pressure, vehicle speed, and step frequency of the moving carrier for information fusion to obtain the trajectories of vehicles and pedestrians. At the same time, the present invention utilizes the structured feature information of roads, stairs, elevators, columns, corners, and walls in the map to constrain the trajectories of vehicles and pedestrians, correct the cumulative error of inertial navigation, and complete the real-time positioning of pedestrians and vehicles with meter-level accuracy. The present invention can reduce the hardware, labor, and other cost requirements for indoor positioning in complex dynamic indoor scenarios, is suitable for positioning in various indoor environments, and is convenient and flexible for use by the user side, that is, it has the advantages of low cost, low environmental dependence, and high flexibility.

[0009] Further, the specific content of S2 is as follows:

[0010] A1. Take the start and end points of the roads in the vector map as nodes and perform a global road search;

[0011] A2. Determine whether the distance from node i to road j is 0. If so, node i and road j are in a subordinate relationship, and proceed to A3. Otherwise, return to A1 and perform a global road search;

[0012] A3. Based on the subordinate relationship, by determining that node i belongs to multiple roads at the same time, obtain the adjacency relationship between roads, and complete the acquisition of the geometric data of the roads and the topological relationship between roads.

[0013] The beneficial effects of the above further solution are as follows: By obtaining the nodes of each road in the vector map, the present invention obtains the geometric data of the roads and the topological relationship between roads, and further obtains the prior information for subsequent dead reckoning trajectory constraint.

[0014] Still further, the specific content of S3 is as follows:

[0015] S301. Based on the known roads where pedestrians and vehicles are currently walking, utilize the signal change characteristics of the built-in sensors of the mobile phone and the vehicle speed change obtained by the vehicle diagnostic system OBDII device to extract the characteristic trajectories of the dead reckoning trajectory;

[0016] S302. Use the heading angle, distance, air pressure change, speed change in the characteristic trajectories and the geometric information and topological relationship in the map data for map matching to obtain the coordinate information of the alternative roads and columns;

[0017] S303. Perform geometric transformation processing of translation, rotation, and scaling on the characteristic trajectory based on the alternative roads and columns. Check the fixed road through the geometric transformation results, and use the fixed road to constrain the offset dead reckoning trajectory within the correct road range.

[0018] The beneficial effect of the above further solution is that: through the observation information of the mobile phone sensor and the vehicle diagnostic system OBDII device, and using various information changes in the characteristic trajectory and map data, the present invention constrains the trajectory, corrects the dead reckoning trajectory to the correct road, and obtains a real-time positioning result.

[0019] Furthermore, the specific content of S301 is as follows:

[0020] B1. On the basis of knowing the current roads where pedestrians and vehicles are walking, extract the straight-line segments of the dead reckoning trajectory using the heading angle and radian change as the straight-line segment characteristic trajectory.

[0021] B2. Extract the elevation change segment of the dead reckoning using the change rate of air pressure with respect to time as the elevation change characteristic trajectory.

[0022] B3. Extract the speed change segment of the dead reckoning using the vehicle diagnostic system OBDII device as the speed change characteristic trajectory, and complete the extraction of the characteristic trajectory of the dead reckoning trajectory.

[0023] The beneficial effect of the above further solution is that: by extracting information such as the straight-line segment plane trajectory, elevation change trajectory, and speed change of the dead reckoning trajectory, the present invention obtains the characteristic trajectory information required for constraining the dead reckoning trajectory.

[0024] Furthermore, the specific content of S302 is as follows:

[0025] C1. Search for alternative roads corresponding to the characteristic trajectory using the current road number and road topology relationship.

[0026] C2. Use the heading angle, distance, air pressure change, and speed change in the characteristic trajectory to perform a second screening on the alternative roads and columns to obtain alternative roads and columns.

[0027] The beneficial effect of the above further solution is that: through the second screening of the alternative roads and columns, the information of the alternative roads and columns corresponding to the characteristic trajectory is determined.

[0028] Furthermore, in S303, using the fixed road to constrain the offset dead reckoning trajectory within the correct road range is specifically as follows:

[0029] D1. Express the coordinate estimation relationship between the dead reckoning trajectory and the map-constrained coordinates as a transformation from one trajectory to another, that is, a similarity transformation between two arc segments. Among them, the relationship satisfied among the dead reckoning trajectory coordinates, the map-constrained coordinate estimations, and the starting coordinates of the arc segments is as follows:

[0030]

[0031] Among them, represents the vehicle coordinate estimation after map constraint at time i, x dr,i , y dr，i represent the dead reckoning trajectory coordinates, x0 and y0 represent the starting coordinates of the current arc segment, k represents the scaling factor, Δx and Δy represent the coordinate translation amounts, and Δα represents the rotation angle;

[0032] D2. Use the coordinate information of the alternative roads and pillars obtained in S302 and the coordinate information in the characteristic trajectory to construct an indirect adjustment model;

[0033] D3. According to the least squares criterion, solve the adjustment results of the estimated parameters related to each group of roads in the indirect adjustment model;

[0034] D4. Conduct a fixed test on the adjustment results, determine whether the test value is greater than the fixed threshold. If so, the fixation is successful, constrain the offset dead reckoning trajectory to the correct road range, and feedback the adjustment results obtained after constraining the dead reckoning trajectory, and enter D5. Otherwise, the fixation fails, then continue to accumulate data and wait for the next fixed test, and return to D1;

[0035] D5. Based on the feedback of the adjustment results, correct the travel trajectories of pedestrians and vehicles to complete the real-time positioning of indoor vehicles and pedestrians.

[0036] The beneficial effects of the above further solution are as follows: Through least squares estimation of the indirect adjustment model established based on the mathematical relationships of different trajectory arc segments and performing a ratio test (ratio test), the dead reckoning trajectory is constrained to the correct road range.

[0037] Furthermore, the construction of the indirect adjustment model is specifically as follows:

[0038] Use the following formula to obtain the relationship between the plane coordinates and the slope and intercept of the road straight line and the constraint between the road azimuth angle and the sampling point heading angle:

[0039]

[0040]

[0041] Among them, k a , k brespectively represent the slope and intercept of the road straight line, j represents the number of samples in this section, i represents the road number, α road 、α i respectively represent the azimuth angle of the road and the heading angle of the sampling point. Among them, based on the coordinate translation amounts Δx and Δy being infinitesimals, the scaling ratio coefficient k is close to 1, and its constraints are:

[0042] 0 - Δx0 = Δx - Δx0 + ε Δx

[0043] 0 - △y0 = △y - △y0 + ε Δy

[0044] 1 - k0 = k - k0 + ε k

[0045] Among them, Δx0 and Δy0 represent the initial translation amounts, ε Δx 、ε Δy represent the translation amount noise, k0 represents the initial scaling amount, and ε k represents the scaling amount noise;

[0046] Based on the above constraints, construct the indirect adjustment model L = BX + V to complete the construction of the indirect adjustment model. Among them, the parameters to be estimated for translating, rotating, and scaling the feature trajectory are:

[0047] X = [△x - △x0 △y - △y0 △α - △α0 k - k0] T

[0048] Among them, L represents various observed quantities, B represents the observation coefficient matrix, V represents the residuals; X represents the parameters to be estimated, and T represents the transpose.

[0049] Furthermore, the specific content of D5 is:

[0050] Perform translation, rotation, and scaling transformation processing on the travel trajectory coordinates of pedestrians and vehicles based on the feedback of the adjustment results;

[0051] Based on the processing results, use the road elevation information in the map data to correct the elevation of the constrained dead reckoning trajectory;

[0052] Based on the feedback of the adjustment results, if the pedestrian is located, correct the real-time heading angle and step length estimation parameters. If the vehicle is located, correct the heading angle in real time to complete the real-time positioning of indoor vehicles and pedestrians at the current epoch.

[0053] The beneficial effect of the above further solution is: Use the adjusted results to perform real-time feedback on the three-dimensional position of the dead reckoning trajectory, correct the corresponding heading angle, coordinate increment, etc., for trajectory constraint at the next moment. Description of the Drawings

[0054] Figure 1 This is the flowchart of the method of the present invention.

[0055] Figure 2 It is a three-dimensional indoor road vector map.

[0056] Figure 3 It is a two-dimensional vector map of the underground parking lot.

[0057] Figure 4 It is a topological relationship acquisition diagram.

[0058] Figure 5 It is a flowchart of pedestrian movement trajectory constraint.

[0059] Figure 6 It is a flowchart of vehicle movement trajectory constraint.

[0060] Figure 7 It is a satellite map of the teaching building.

[0061] Figure 8 It is a map of pedestrian dead reckoning results.

[0062] Figure 9 It is a schematic diagram of the pedestrian trajectory after map constraint (X-Y direction).

[0063] Figure 10 It is a schematic diagram of the pedestrian trajectory after map constraint (X-Z direction).

[0064] Figure 11 It is a schematic diagram of the pedestrian trajectory after map constraint (Y-Z direction).

[0065] Figure 12 It is a schematic diagram of the second basement floor of the underground parking lot.

[0066] Figure 13 It is a real-time vehicle trajectory map of Experiment 1.

[0067] Figure 14 It is a real-time vehicle trajectory map of Experiment 2. Detailed implementation manners

[0068] The following describes the detailed implementation manners of the present invention to facilitate those skilled in the art of this technology to understand the present invention. However, it should be clear that the present invention is not limited to the scope of the detailed implementation manners. For those of ordinary skill in the art of this technology, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions and creations using the concept of the present invention are within the scope of protection.

[0069] Embodiment

[0070] As Figure 1 shown, the present invention provides an indoor vehicle and pedestrian real-time positioning method, and its implementation method is as follows:

[0071] S1. As shown in Figure 2 and Figure 3 , obtain the vector map in the test room;

[0072] S2. Take the start and end points of the roads in the vector map as nodes, and obtain the geometric data of the roads and the topological relationship between the roads through the node coordinates. The implementation method is as follows:

[0073] A1. Take the start and end points of the roads in the vector map as nodes and perform a global road search;

[0074] A2. Determine whether the distance from node i to road j is 0. If so, node i and road j are in a subordinate relationship, and go to A3. Otherwise, return to A1 and perform a global road search;

[0075] A3. Based on the subordinate relationship, by determining that node i belongs to multiple roads at the same time, obtain the adjacency relationship between the roads, and complete the acquisition of the geometric data of the roads and the topological relationship between the roads.

[0076] In this embodiment, as shown in Figure 4 , take the start and end points of the roads in the map as nodes, and obtain the geometric data of the roads and the topological relationship between the roads through the node coordinates.

[0077] Structurize the geometric information and topological relationship of the indoor map, and the result is shown in Table 1 below. Table 1 is the road geometry and topology data table.

[0078] Table 1

[0079]

[0080] S3. Based on the topological relationship, on the basis of knowing the roads where the pedestrians and vehicles are currently walking, constrain the offset dead reckoning trajectory to the correct road range through road matching, and complete the real-time positioning of indoor vehicles and pedestrians. The implementation method is as follows:

[0081] S301. On the basis of knowing the roads where the pedestrians and vehicles are currently walking, use the signal change characteristics of the built-in sensors of the mobile phone and the vehicle speed change obtained by the vehicle diagnostic system OBDII device to extract the characteristic trajectory of the dead reckoning trajectory. The implementation method is as follows:

[0082] B1. On the basis of knowing the roads where the pedestrians and vehicles are currently walking, use the heading angle and radian change to extract the straight line segment of the dead reckoning trajectory as the straight line segment characteristic trajectory;

[0083] B2. Use the change rate of air pressure with respect to time to extract the elevation change segment of the dead reckoning as the elevation change characteristic trajectory;

[0084] B3. Use the on-board diagnostic system OBDII device to extract the speed change segments of dead reckoning as the characteristic trajectory of speed change, and complete the extraction of the characteristic trajectory for the dead reckoning trajectory.

[0085] S302. Use the heading angle, distance, barometric pressure change, speed change in the characteristic trajectory, and the geometric information and topological relationship in the map data for map matching to obtain the coordinate information of the alternative roads and poles. The implementation method is as follows:

[0086] C1. Search for the alternative roads corresponding to the characteristic trajectory using the current road number and road topological relationship;

[0087] C2. Use the heading angle, distance, barometric pressure change, speed change in the characteristic trajectory to perform a second screening on the alternative roads and poles to obtain the alternative roads and poles;

[0088] S303. Perform geometric transformation processing of translation, rotation, and scaling on the characteristic trajectory according to the alternative roads and poles, check the fixed road through the geometric transformation results, and use the fixed road to constrain the offset dead reckoning trajectory within the correct road range. Specifically:

[0089] Express the coordinate estimation relationship between the dead reckoning trajectory and the map constraint as a similarity transformation from one trajectory to another, that is, the similarity transformation of two arc segments; among them, the relationship satisfied among the dead reckoning trajectory coordinates, the coordinate estimation value after map constraint, and the starting coordinates of the arc segment is:

[0090]

[0091] Among them, represents the vehicle coordinate estimation value after map constraint at time i, x dr，i , y dr，i represent the dead reckoning trajectory coordinates, x0, y0 represent the starting coordinates of the current arc segment, k represents the scaling ratio coefficient, Δx, Δy represent the coordinate translation amount, and Δα represents the rotation angle;

[0092] D2. Use the coordinate information of the alternative roads and poles obtained in S302 and the coordinate information in the characteristic trajectory to construct an indirect adjustment model. The implementation method is as follows:

[0093] Use the following formula to obtain the relationship between the plane coordinates and the slope and intercept of the road straight line and the constraint between the road azimuth and the sampling point heading angle:

[0094]

[0095]

[0096] Among them, k a , k brespectively represent the slope and intercept of the road straight line, j represents the number of samples of this section, i represents the road number, α road and α i respectively represent the azimuth angle of the road and the heading angle of the sampling point. Among them, based on the coordinate translation amounts Δx and Δy being infinitesimals, the scaling ratio coefficient k is close to 1, and its constraints are:

[0097] 0 - △x0 = △x - △x0 + ε Δx

[0098] 0 - △y0 = △y - △y0 + ε △y

[0099] 1 - k0 = k - k0 + ε k

[0100] Among them, Δx0 and Δy0 represent the initial translation amounts, ε Δx and ε Δy represent the translation amount noise, k0 represents the initial scaling amount, and ε k represents the scaling amount noise;

[0101] Based on the above constraints, construct the indirect adjustment model L = BX + V to complete the construction of the indirect adjustment model. Among them, the parameters to be estimated for translating, rotating, and scaling the feature trajectory are:

[0102] X = [△x - Δx0 △y - △y0 △α - △α0 k - k0] T

[0103] Among them, L represents various observed quantities, B represents the observation coefficient matrix, V represents the residuals, X represents the parameters to be estimated, and T represents the transpose;

[0104] D3. According to the least - squares criterion, solve the adjustment results of each parameter to be estimated related to each group of roads in the indirect adjustment model;

[0105] D4. Conduct a fixed - value test on the adjustment results to determine whether the test value is greater than the fixed threshold. If so, the fixation is successful, constrain the offset dead - reckoning trajectory to the correct road range, and feedback the adjustment results obtained after constraining the dead - reckoning trajectory, and enter D5. Otherwise, the fixation fails, then continue to accumulate data and wait for the next fixed - value test, and return to D1;

[0106] D5. Based on the feedback of the adjustment results, correct the travel trajectories of pedestrians and vehicles to complete the real - time positioning of indoor vehicles and pedestrians. The implementation method is as follows:

[0107] Conduct translation, rotation, and scaling transformation processing on the travel trajectory coordinates of pedestrians and vehicles based on the feedback of the adjustment results;

[0108] Based on the processing result, the elevation of the constrained dead reckoning trajectory is corrected using the road elevation information in the map data;

[0109] Based on the feedback of the adjustment result, if a pedestrian is located, the real-time heading angle and step size estimation parameters are corrected. If a vehicle is located, the heading angle is corrected in real time to complete the real-time positioning of indoor vehicles and pedestrians at the current epoch.

[0110] Translating, rotating, and scaling the feature trajectory according to the adjustment result is to obtain the two-dimensional coordinates of the constrained trajectory; according to the road elevation information is to obtain the elevation information of the trajectory, that is, to obtain the three-dimensional coordinates of the trajectory; correcting the heading angle, etc. is to prepare for trajectory constraint in the next epoch and obtain the corresponding initial parameters.

[0111] In this embodiment, the walking ranges of pedestrians and vehicles are always within the road range and cannot cross walls or jump roads, having good coherence. And the topological structure between roads is recorded in the map data, that is, the belonging relationship between roads and nodes and the connection relationship between roads and roads. Therefore, through this feature, based on the currently traveled roads of pedestrians and vehicles, the road possibilities corresponding to the roads to be matched can be predicted, and the offset dead reckoning trajectory can be constrained to the correct road range through road matching. Specifically:

[0112] Step 1: Extract the feature trajectory of the dead reckoning trajectory using the signal change characteristics of the built-in sensors of the mobile phone and the vehicle speed change obtained by the vehicle diagnostic system OBDII device.

[0113] Step 2: Perform map matching using the heading angle, distance, barometric pressure change, speed change in the feature trajectory and the geometric information and topological relationship in the map data to obtain alternative roads and columns.

[0114] Step 3: Perform geometric transformations of translation, rotation, and scaling on the feature trajectory according to the alternative roads and columns, and perform least squares estimation to obtain the fixed road information.

[0115] Step 4: Use the adjustment result of the fixed road to constrain the dead reckoning trajectory to the correct road, and correct various parameters in the dead reckoning constraint model for the calculation of the next epoch.

[0116] In this embodiment, in Step 1, the specific method for extracting the feature trajectory of the dead reckoning trajectory is:

[0117] (1) Extract the straight line segments of the dead reckoning trajectory using the heading angle and radian change as the straight line segment feature trajectory.

[0118] (2) Extract the elevation change segment of the dead reckoning using the rate of change of barometric pressure with respect to time as the elevation change feature trajectory.

[0119] (3) Use the on-board diagnostic system OBDII device of the vehicle to extract the speed change segments of dead reckoning as the speed change characteristic trajectory.

[0120] In this embodiment, in step 2, the specific method of road matching is as follows:

[0121] (1) Search for alternative roads that the characteristic trajectory may correspond to by using the current road number and road topology relationship.

[0122] (2) Use the heading angle, distance, barometric change characteristics, and speed of the characteristic trajectory to perform a second screening on the alternative roads and columns.

[0123] In this embodiment, in step 3, the specific method of fixing the dead reckoning trajectory is as follows:

[0124] (1) Express the relationship between the dead reckoning trajectory and the coordinate estimation value after map constraint as a similarity transformation from one trajectory to one trajectory, that is, two arc segments. Therefore, an indirect adjustment model can be used to construct unknown parameters of translation amount, rotation amount, and scaling factor, and the observed value is the coordinate of the dead reckoning trajectory after constraint, that is:

[0125] X = [Δx Δy Δa k] T

[0126] Among them, X represents the parameter to be estimated, Δx and Δy represent the coordinate translation amount, Δa represents the rotation amount, and k represents the scaling factor.

[0127] (2) The constraints are the relationship between the plane coordinates and the slope and intercept of the road straight line, and the constraint between the road azimuth angle and the sampling point heading angle:

[0128]

[0129]

[0130] Among them, k a 、k b represent the slope and intercept of the road straight line respectively, j represents the number of samples in this section, i represents the road number, α road 、α i represent the road azimuth angle and the sampling point heading angle respectively. Among them, based on the coordinate translation amount Δx, yx is a small amount, and the scaling factor k is close to 1, and its constraint is:

[0131] 0 - △x0 = △x - △x0 + ε Δx

[0132] 0 - △y0 = △y - △y0 + ε Δy

[0133] 1 - k0 = k - k0 + ε k

[0134] where, Δx0 and Δy0 represent the initial translation amounts, and ε Δx , ε Δy represent the translation amount noise, k0 represents the initial scaling amount, and ε k represents the scaling amount noise.

[0135] (3) Put the feature trajectory and the corresponding alternative roads into the indirect adjustment model, and obtain the adjustment results of each group of roads according to the least squares criterion.

[0136] (4) Conduct a fixation test on all the adjustment results. If the test value is greater than the fixation threshold, the fixation is successful; if the fixation fails, continue to accumulate data and wait for the next fixation.

[0137] In this embodiment, in step 4, the specific method for fixing the traveling trajectories of pedestrians and vehicles is as follows:

[0138] (1) Use the adjustment results of the fixed roads to perform translation, rotation, and scaling transformations on the dead reckoning trajectory, and constrain the dead reckoning trajectory within the correct road range.

[0139] (2) Use the road elevation information in the map data to correct the elevation of the constrained dead reckoning trajectory.

[0140] (3) Feed the adjustment results of the fixed roads back to the dead reckoning model. If it is pedestrian positioning, correct the real-time heading angle and step length estimation parameters (the traveling distances Δx and Δy between adjacent epochs of the carrier); if it is vehicle positioning, correct the heading angle in real time to improve the subsequent positioning accuracy.

[0141] In this embodiment, as Figure 5 and Figure 6 shown, the implementation process of constraining the traveling trajectories of pedestrians and vehicles can be summarized as follows:

[0142] During the traveling processes of pedestrians and vehicles, fix the smart phones to the pedestrians and vehicles respectively as strap-down inertial navigation systems, and use the angular motion parameters and linear motion parameters of the pedestrians and vehicles relative to space measured by the built-in accelerometers, gyroscopes, barometers of the smart phones, and the vehicle speed information of the vehicle diagnostic system OBDII device. Through dead reckoning, obtain the two-dimensional coordinate estimations of the pedestrians and vehicles in real time. By fusing the relationship between air pressure and elevation and the vehicle speed correction model (the formula is as follows), the three-dimensional coordinate estimations of the pedestrians and vehicles during the traveling processes can be obtained in real time:

[0143]

[0144]

[0145] Among them, P and P0 respectively represent the actual atmospheric pressure and the standard atmospheric pressure at sea level (usually taken as 101325 Pa), h and H respectively represent the actual altitude and the atmospheric thickness (usually taken as 8400 m); coe represents the speed correction coefficient, L represents the true distance traveled by the vehicle, v represents the real-time speed of the vehicle. The three-dimensional coordinate estimations accumulated over a period of time can form a trajectory. The characteristic trajectory in the pedestrian trajectory is extracted through the heading angle and elevation change, and the characteristic trajectory is map-matched using information such as the road topology, elevation, azimuth angle, and distance in the prior map to obtain the alternative roads and columns that the characteristic trajectory may correspond to.

[0146] The characteristic trajectory, all alternative roads and columns are sequentially put into the indirect adjustment model, and a fixed test is performed on all adjustment results to fix the road. The two-dimensional coordinate estimations of all current pedestrians and vehicles are subjected to overall translation, rotation, and scaling transformations through the adjustment results of the fixed road to obtain the coordinate estimations that conform to the correct road.

[0147] When the characteristic trajectory has not reached the adjustment opportunity, the dead reckoning, barometric pressure and elevation relationship model are always used to calculate the three-dimensional coordinate estimations of the pedestrians and vehicles; when the characteristic trajectory reaches the adjustment opportunity, that is, when the straight line segment extracted from the real-time dead reckoning trajectory satisfies the following formula, and after successfully fixing the road, all the current unconstrained three-dimensional coordinate estimations are corrected as a whole. And the adjustment results of the fixed road are used to correct the current real-time heading angle and step length estimation model parameters to improve the two-dimensional coordinate estimations obtained by real-time dead reckoning. Among them, the conditions satisfied during the adjustment calculation are as follows:

[0148]

[0149] |L last |>b

[0150] Among them, L last respectively represent the maximum heading angle, minimum heading angle of each straight line segment and the driving length of the nearest straight line segment at the current moment; a and b are the corresponding thresholds set according to the azimuth angle difference between roads in the scene and the travel road lengths of indoor pedestrians or vehicles.

[0151] In summary, compared with the prior art, the present invention has the following advantages and beneficial effects:

[0152] 1. The construction and maintenance cost of the server is low. It does not rely on the erection of various signal base stations (such as pseudolites, UWB, WIFI, Bluetooth, etc.), reduces the construction and maintenance costs, and only needs to collect indoor maps or use existing indoor maps, which is convenient for promotion in different indoor environments.

[0153] 2. Low software and hardware usage costs for the user side. No additional sensors need to be installed on the user side. Only low-cost sensors integrated in smartphones can be used for data collection, processing, and display, facilitating popularization and application among the general public.

[0154] 3. The positioning accuracy, reliability, and real-time performance can all meet the requirements of indoor navigation and positioning for people and vehicles. It does not rely on 4G or 5G communication networks. By integrating sensors such as maps, low-cost inertial navigation, barometers, and vehicle OBDII diagnostic devices, real-time navigation and positioning with centimeter-level accuracy for pedestrians and centimeter-level accuracy for vehicle parking spaces can be achieved.

[0155] The present invention will be further described below in conjunction with the test site.

[0156] 1. Indoor pedestrian positioning:

[0157] The test site is a five-story teaching building. The satellite map is as Figure 7 shown. As can be seen from the figure, the plane presents an "A" shape, and the floors are connected by stairs and elevators. The main building of the building is divided into two parts: the teaching area and the office area. The same floor height in different areas is different and is connected by a skybridge on the third floor. The five entrances to the indoor of this teaching building are marked with red circles in the figure.

[0158] The experiment was set up for pedestrians to walk randomly indoors under the condition of known initial position and heading angle. In the experimental results, the traditional pedestrian dead reckoning results are as Figure 8 shown, while the pedestrian trajectory results after map constraint are as Figure 9 , Figure 10 and Figure 11 shown.

[0159] 2. Indoor vehicle positioning:

[0160] The test site is the underground parking lot of Runyang Shuangtie Square in Pi District, Chengdu City, Sichuan Province. The floor plan of the parking lot is as Figure 12 shown. Among all kinds of elements in the figure, the thick black solid line is the wall, the light solid line is the center line of the road, and the red dot is the load-bearing column.

[0161] In the experiment, the smartphone was used to obtain the vehicle angle change information in real time, and the OBDII diagnostic device was used to obtain the vehicle speed information via Bluetooth in real time. The experimental set travel trajectory was a random route, and complex vehicle driving such as driving in circles in large and small areas, starting and stopping after two vehicles met, and detouring on unconventional roads was carried out. The vehicle trajectory results after map constraint are shown as Figure 13 and Figure 14 shown.

[0162] It can be clearly seen from the above tests that in pedestrian navigation, under the influence of heading angle drift and step length cumulative error, the pedestrian trajectory significantly deviates from the map roads. After trajectory correction based on map constraints, the heading angle drift and step length error are effectively reduced. The heading angle is more in line with the azimuth angle of each road, and the walking distance is also within the range of the road, which is more in line with the actual walking situation on site. Similarly, in vehicle navigation, whether it is various regional circles or detours on unconventional routes, through map constraints, the algorithm can accurately and quickly identify changes in vehicle state, correct the vehicle's traveling trajectory in real time, and obtain a positioning result with a precision of meters.

Claims

1. An indoor vehicle and pedestrian real-time positioning method, characterized in that, It includes the following steps: S1. Obtain the vector map in the test room; S2. Take the start and end points of the roads in the vector map as nodes, and obtain the geometric data of the roads and the topological relationship between the roads through the node coordinates; S3. Based on the topological relationship, on the basis of knowing the roads currently traveled by pedestrians and vehicles, constrain the offset dead reckoning trajectory to the correct road range through road matching, and complete the real-time positioning of indoor vehicles and pedestrians.

2. The real-time indoor vehicle and pedestrian positioning method according to claim 1, characterized in that The specific content of S2 is as follows: A1. Take the start and end points of the roads in the vector map as nodes, and perform a global road search; A2. Judge whether the distance from node i to road j is 0. If so, node i and road j are in a belonging relationship, and enter A3. Otherwise, return to A1 and perform a global road search; A3. Based on the belonging relationship, by determining that node i belongs to multiple roads at the same time, obtain the adjacency relationship between roads, and complete the acquisition of the geometric data of the roads and the topological relationship between the roads.

3. The indoor vehicle and pedestrian real-time positioning method according to claim 1, wherein, The specific content of S3 is as follows: S301. On the basis of knowing the roads currently traveled by pedestrians and vehicles, extract the characteristic trajectory of the dead reckoning trajectory by using the signal change characteristics of the built-in sensors of the mobile phone and the vehicle speed change obtained by the vehicle diagnostic system OBDII device; S302. Use the heading angle, distance, air pressure change, speed change in the characteristic trajectory and the geometric information and topological relationship in the map data for map matching to obtain the coordinate information of the alternative roads and columns; S303. Perform geometric transformation processing of translation, rotation and scaling on the characteristic trajectory according to the alternative roads and columns, check the fixed roads through the geometric transformation results, and use the fixed roads to constrain the offset dead reckoning trajectory to the correct road range.

4. The indoor vehicle and pedestrian real-time positioning method according to claim 3, wherein, The specific content of S301 is as follows: B1. On the basis of knowing the roads currently traveled by pedestrians and vehicles, extract the straight line segment of the dead reckoning trajectory by using the heading angle and radian change as the straight line segment characteristic trajectory; B2. Extract the elevation change segment of the dead reckoning by using the change rate of air pressure relative to time as the elevation change characteristic trajectory; B3. Use the vehicle diagnostic system OBDII device to extract the speed change segment of the dead reckoning as the speed change characteristic trajectory, and complete the extraction of the characteristic trajectory of the dead reckoning trajectory.

5. The indoor vehicle and pedestrian real-time positioning method according to claim 3, wherein, The specific content of S302 is as follows: C1. Search for alternative roads corresponding to the characteristic trajectory by using the current road number and road topology relationship; C2. Use the heading angle, distance, air pressure change, and speed change in the characteristic trajectory to perform a second screening on the alternative roads and columns to obtain the alternative roads and columns.

6. The indoor vehicle and pedestrian real-time positioning method according to claim 3, characterized in that In S303, using the fixed road to constrain the offset dead reckoning trajectory to the correct road range, the specific content is as follows: D1. Express the coordinate estimation relationship between the dead reckoning trajectory and the map constraint as a similarity transformation from one trajectory to another trajectory, that is, two arc segments; the relationship satisfied among the dead reckoning trajectory coordinates, the coordinate estimation after map constraint, and the starting coordinates of the arc segments is: Among them, represents the estimated vehicle coordinate after map constraint at time i, x dr，i , y dr，i represents the dead reckoning trajectory coordinates, x0 and y0 represent the starting coordinates of the current arc segment, k represents the scaling factor, Δx and Δy represent the coordinate translation amounts, and Δα represents the rotation angle; D2. Use the coordinate information of the alternative roads and columns obtained in S302 and the coordinate information in the characteristic trajectory to construct an indirect adjustment model; D3. Solve the adjustment results of each estimated parameter related to each group of roads in the indirect adjustment model according to the least squares criterion; D4. Conduct a fixed test on the adjustment results to determine whether the test value is greater than the fixed threshold. If so, the fixation is successful. Constrain the offset dead reckoning trajectory to the correct road range, and feedback the adjustment results obtained after the dead reckoning trajectory is constrained, and enter D5. Otherwise, the fixation fails, continue to accumulate data and wait for the next fixed test, and return to D1; D5. Based on the feedback of the adjustment results, correct the coordinates of the pedestrian and vehicle travel trajectories to complete the real-time positioning of indoor vehicles and pedestrians.

7. The indoor vehicle and pedestrian real-time positioning method according to claim 6, characterized in that, The construction of the indirect adjustment model is specifically as follows: Using the following formula, obtain the relationship between the plane coordinates and the slope and intercept of the road straight line, and the constraint between the road azimuth and the sampling point heading angle: where k a and k b represent the slope and intercept of the road straight line respectively, j represents the number of samples of this section, i represents the road number, and α road and α i represent the road azimuth angle and the heading angle of the sampling point respectively. Among them, based on the coordinate translation amounts Δx and Δy being infinitesimals, the scaling factor k is close to 1, and its constraint is: 0 - △x0 = △x - △x0 + ε△x 0 - Δy0 = Δy - Δy0 + ε Δy 1 - k0 = k - k0 + ε k where Δx0 and Δy0 represent the initial translation amounts, ε Δx , ε Δy represent the translation amount noise, k0 represents the initial scaling amount, and ε k represents the scaling amount noise; Based on the above constraints, construct the indirect adjustment model L = BX + V to complete the construction of the indirect adjustment model. Among them, the estimated parameters for translating, rotating, and scaling the feature trajectory are: X = [Δx - Δx0 Δy - Δy0 Δα - Δα0 k - k0] T Among them, L represents various observed quantities, B represents the observation coefficient matrix, V represents the residual, X represents the estimated parameter, and T represents the transpose.

8. The indoor vehicle and pedestrian real-time positioning method according to claim 6, characterized in that, The specific content of D5 is as follows: Conduct translation, rotation, and scaling transformation processing on the coordinates of the pedestrian and vehicle travel trajectories based on the feedback of the adjustment results; Based on the processing results, use the road elevation information in the map data to correct the elevation of the constrained dead reckoning trajectory; Based on the feedback of the adjustment results, if it is a pedestrian positioning, correct the real-time heading angle and step length estimation parameters. If it is a vehicle positioning, correct the heading angle in real time to complete the real-time positioning of indoor vehicles and pedestrians in the current epoch.

Images