A unified navigation method and device for above-ground and underground

Abstract

The embodiment of the present invention relates to a unified navigation method and device for above-ground and underground navigation, the method comprising the steps of: establishing a unified coordinate system above-ground and underground; planning underground paths based on the unified coordinate system; and visually expressing the planned paths. The technical solution of the embodiment of the present invention establishes an absolute coordinate system by integrating and converting different coordinate system positioning into the 2000 National Geodetic Coordinate System. Only one available angle vector is required for more accurate positioning, and the availability is higher under special conditions. At the same time, the amount of calculation is reduced except for the start of positioning and the participation of new system signals in positioning, and the power consumption of the equipment is reduced. When a new system is added, it is also easier to further convert the relative coordinates of the original system into absolute coordinates based on the existing positioning, thereby realizing seamless switching between positioning systems. For special areas where external signals cannot be received and base stations cannot be configured, the absolute coordinates can be determined through image information, NFC tags, etc., so as to calibrate and optimize terminal positioning.

Description

Technical Field

[0001] The embodiments of the present invention relate to the technical field of navigation positioning and location services, and in particular to a unified above-ground and underground navigation method and device. Background Art

[0002] With the increasing popularity and sophistication of mobile Internet of Things devices, the demand for industrial intelligence has gradually changed from "usable enough" to "easy to use, efficient, and scalable."

[0003] Among them, as the earliest popular navigation system, the stability and accuracy are still limited by the strength of satellite signals. In scenes with poor signals such as complex terrain, indoor places, and special work places, it will be difficult to locate or even impossible to locate. Existing solutions to this problem mostly rely on different positioning methods and use two or more systems for positioning, which causes the terminal to receive multiple sets of positioning coordinates and need to be converted at any time to determine the positioning position. This problem has caused an increase in the amount of calculation and a significant increase in the amount of positioning calculations for multiple systems. At the same time, it is difficult for the terminal device to roughly calculate the current coordinates based on the last valid coordinates when the signal is temporarily lost. These problems are great challenges to the terminal power and positioning stability in special working environments such as mines, underground transportation and other places with poor signal environments. There is a high risk that the positioning system will not work properly when an unexpected situation occurs. Summary of the invention

[0004] Based on the above-mentioned situation of the prior art, the purpose of the embodiments of the present invention is to provide a unified above-ground and underground navigation method and device, which realizes seamless above-ground and underground positioning on the basis of establishing a unified coordinate system and then constructs and connects above-ground and underground path networks, thereby realizing seamless switching between positioning systems and improving the accuracy and stability of unified above-ground and underground navigation.

[0005] To achieve the above object, according to one aspect of the present invention, a unified above-ground and underground navigation method is provided, comprising the steps of:

[0006] Establish a unified coordinate system above and below ground;

[0007] Underground path planning based on a unified coordinate system;

[0008] Visualize the planned path.

[0009] Furthermore, establishing a unified coordinate system above and below ground includes:

[0010] Obtain the three-dimensional coordinates of the outdoor first coordinate system;

[0011] Converting the outdoor first coordinate system into a second coordinate system, where the second coordinate system is an absolute coordinate system;

[0012] Calculate the absolute coordinates of the ground and underground connection area and the points to be located in the second coordinate system;

[0013] Calculate the absolute coordinates of the point to be located in the underground area in the second coordinate system.

[0014] Furthermore, the calculation of the above-ground and underground connection areas and the absolute coordinates of the points to be located in the second coordinate system include:

[0015] Calculate the inertial position change of the point to be positioned through the acceleration and angular velocity of the point to be positioned;

[0016] The absolute coordinates of the point to be located at the second coordinate are obtained according to the previous inertial position coordinates and the inertial position change of the point to be located.

[0017] Furthermore, the calculation of the absolute coordinates of the point to be located in the underground area in the second coordinate system includes:

[0018] Obtain the absolute coordinates of each ground and underground connection point in the second coordinate system;

[0019] Calculate the distance from the point to be located to the main base station and the angle from the main base station;

[0020] Calculate the coordinates of the point to be located in the second coordinate system according to the distance and angle;

[0021] Calculate the mark of the point to be located in the time dimension according to the timestamp of the signal sent each time the point to be located moves;

[0022] The absolute coordinates of the point to be located in the second coordinate system are obtained based on the coordinates of the point to be located and the mark of the point to be located in the time dimension.

[0023] Furthermore, the establishment of a unified above-ground and underground coordinate system further includes:

[0024] Error calibration is performed on the absolute coordinates of the points to be positioned in the ground and underground connection area in the second coordinate system.

[0025] Furthermore, the underground path planning based on the unified coordinate system includes:

[0026] Construct an underground path network based on a unified coordinate system;

[0027] In an underground path network, the shortest path is calculated for navigation tasks that start in an above-ground or underground area and end in an underground or above-ground area.

[0028] Furthermore, the construction of the underground path network includes:

[0029] Abstracting the underground facility into a polyhedron, and obtaining coordinates of the vertices of the polyhedron in the unified coordinate system;

[0030] Select several points in the ground area close to the underground of the polyhedron as element points, select several points in the passable area as first path points, and select several points in the ground area near the ground-underground junction area as second path points;

[0031] Connect all element points and path points to form a path network topology diagram;

[0032] According to the connection between the first path point and the second path point, the path points are divided into connected path points and disconnected path points.

[0033] Further, the calculating the shortest path includes:

[0034] Obtaining a set of polyhedral coordinates of multiple underground facilities;

[0035] Determine whether the end point is located in any polyhedron. If so, lock the first polyhedron where the end point is located; if not, find the path point closest to the midpoint in the first path point array as the end point;

[0036] Calculate the shortest distances from the starting point and the end point to the connected path points, as well as the shortest distance from the starting point to the end point;

[0037] Mark all the points passed in the shortest distance from the starting point to the end point and connect them to get the shortest path from the starting point to the end point.

[0038] According to another aspect of the present invention, there is provided a unified above-ground and underground navigation device, comprising:

[0039] Unified coordinate system establishment module, used to establish a unified coordinate system above ground and underground;

[0040] Underground path planning module, used for underground path planning based on a unified coordinate system;

[0041] The visualization expression module is used to visualize the planned path.

[0042] In summary, the embodiment of the present invention provides a unified navigation method and device for ground and underground, the method comprising the steps of: establishing a unified coordinate system for ground and underground; planning underground paths based on the unified coordinate system; and visually expressing the planned paths. The technical solution of the embodiment of the present invention establishes an absolute coordinate system by integrating and converting different coordinate system positioning into the 2000 National Geodetic Coordinate System, and only requires an available angle vector for more accurate positioning, and has higher availability under special conditions; at the same time, it reduces the amount of calculation when the positioning starts and the new system signal participates in the positioning, and reduces the power consumption of the equipment; when the new system is added, it is also easier to further convert the relative coordinates of the original system into absolute coordinates based on the existing positioning, thereby realizing seamless switching between positioning systems. For special areas where external signals cannot be received and base stations cannot be configured, the absolute coordinates can be determined by image information, NFC tags, etc., so as to further calibrate and optimize the terminal positioning. The present invention has the following beneficial technical effects: it makes up for the shortcomings of the existing technology that ground and underground navigation cannot be integrated, and that there are two types of positioning data in the ground and underground interactive areas at the same time, and the accuracy of the two types of data is affected by the environment and the switching error is large. Through the establishment of inertial navigation technology and a unified absolute coordinate system, accurate ground and underground positioning is ensured while achieving seamless switching and navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 is a flow chart of a unified above-ground and underground navigation method provided by an embodiment of the present invention;

[0044] Figure 2 It is a schematic diagram for calculating the distance from the point to be located to the main base station and the angle from the main base station;

[0045] Figure 3 It is a schematic diagram of an integrated above-ground and underground path network in a specific example of the present invention. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of well-known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.

[0047] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in one or more embodiments of the present invention should be understood by people with ordinary skills in the field to which the present disclosure belongs. The "first", "second" and similar words used in one or more embodiments of the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. "Include" or "comprise" and other similar words mean that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and other similar words are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect.

[0048] The technical solution of the present invention is described in detail below with reference to the accompanying drawings. The embodiment of the present invention provides a unified navigation method for above-ground and underground. Figure 1 A flow chart of the method is shown, as Figure 1 As shown, the following steps are included:

[0049] S102, establishing a unified ground and underground coordinate system. In this embodiment of the present invention, the establishment of a unified ground and underground coordinate system is achieved based on the Beidou CORS system, inertial navigation technology and ultra-wideband technology (UWB), which can specifically include the following steps:

[0050] S1022. Obtain the three-dimensional coordinates of the outdoor first coordinate system. In this embodiment of the present invention, the outdoor first coordinate system can adopt a coordinate system based on the Beidou CORS system. Under the condition that the Beidou CORS system is completed, the network carrier phase differential technology RTK system based on the China Beidou Navigation Satellite System (BeiDou Navigation Satellite System, hereinafter referred to as "BDS") is used to obtain the three-dimensional coordinates (X, Y, Z) and speed V of outdoor users all day long. t In this step, speed and time information can also be obtained, where the speed is the moving speed of the point to be located, and the time information can be used to record the position of the point to be located in real time for navigation visualization after the system is built.

[0051] S1024, converting the outdoor first coordinate system into a second coordinate system, which is an absolute coordinate system. In this embodiment of the present invention, the second coordinate system is the 2000 National Geodetic Coordinate System, which is a geodetic coordinate system with high accuracy, dynamicity and practicality. In this step, the relative coordinates obtained based on the Beidou CORS system are converted to the second coordinate system, i.e., the 2000 National Geodetic Coordinate System, using the Bursa model (B model). The conversion process can be performed according to the following steps:

[0052] Assume that the coordinates in the first coordinate system are (X1, Y1, Z1), and the coordinates in the second coordinate system (2000 National Geodetic Coordinate System) are (X2, Y2, Z2). The difference between the origins of the first coordinate system and the second coordinate system, i.e., the three translation parameters, are (Δ X ,Δ Y ,Δ Z ), three rotation parameters (ε X ,ε Y ,ε Z ), the rotation parameter is the angle by which the three coordinate axes are rotated in sequence to make the XYZ axes of the two spatial rectangular coordinate systems coincide, and a scale transformation parameter m, that is, the ratio of the lengths of the same straight line in the first coordinate system and the second coordinate system, which is used to achieve the proportional conversion of the scale. Usually, the value of m is almost equal to 1;

[0053] According to the Bursa model, the transformation formula is:

[0054]

[0055] Thus, the absolute coordinates (X2, Y2, Z2) of each outdoor position on the ground in the second coordinate system (2000 National Geodetic Coordinate System) can be obtained.

[0056] S1026. Calculate the absolute coordinates of the point to be positioned in the second coordinate system in the ground-underground connection area. Because there is a slight error between the ground and underground positioning methods in the ground-underground connection area, that is, the positioning of the ground-underground connection area by the Beidou positioning method and the underground UWB positioning method is inaccurate and has a large error. When the error exceeds 10%, the part is identified as the connection area and inertial navigation is used for positioning. Inertia refers to acceleration, which refers to the use of inertial elements (accelerometers) to measure the acceleration of the vehicle itself, and the speed and position are obtained through integration and calculation, so as to achieve the purpose of navigation and positioning of the vehicle. The specific steps are as follows:

[0057] S10261. Calculate the inertial position change of the point to be positioned by integrating the acceleration and angular velocity of the point to be positioned over time.

[0058] S10262. Obtain the absolute coordinates of the point to be located at the second coordinate according to the previous inertial position coordinates and the inertial position change of the point to be located.

[0059] In the above steps S10261 and S10262, the current absolute coordinate system (navigation coordinate system), that is, the second coordinate system, can be first converted to the inertial coordinate system of the point to be located. The inertial coordinate system is generated to simplify the transformation from the world coordinate system to the object coordinate system. The origin of the inertial coordinate system coincides with the origin of the object coordinate system, and the axes of the inertial coordinate system are parallel to the axes of the world coordinate system. After the inertial coordinate system is introduced, the object coordinate system only needs to be rotated to be converted to the inertial coordinate system, and the inertial coordinate system only needs to be translated to be converted to the world coordinate system. The rotations of α, θ around the X, Y, and Z axes are measured, respectively. α, θ, It means that by rotating the three coordinate axes in sequence by the specified angle, the XYZ axes of the two spatial rectangular coordinate systems can be made to coincide. Then the coordinates of a point in the current inertial coordinate system (x', y', z'):

[0060]

[0061] The accelerations in the X, Y, and Z axes in the inertial coordinate system are a X ,a Y ,a Z , this acceleration can be measured by the inertial element in inertial navigation and according to the following formula:

[0062]

[0063] The new coordinates of the point in the coordinate system (X' t ,Y' t ,Z' t ):

[0064]

[0065] Then convert the point to the absolute coordinates in the second coordinate system (2000 National Geodetic Coordinate System) to obtain:

[0066]

[0067] Through the above steps, the absolute coordinates (x t ,y t ,z t ).

[0068] Since each ground position can be converted into the second coordinate system (2000 National Geodetic Coordinate System), the navigation coordinate system in the inertial navigation, that is, the 2000 National Geodetic Coordinate System adopted in this embodiment of the present invention, the coordinates of the ground and underground connection areas obtained after the inertial navigation conversion are the absolute coordinates in the second coordinate system.

[0069] S1028, calculate the absolute coordinates of the point to be located in the underground area in the second coordinate system. In this embodiment of the present invention, the positioning of the underground position adopts ultra-wideband technology (UWB) with multiple advantages such as relatively accurate positioning and low power consumption to construct an indoor positioning coordinate system. In this embodiment of the present invention, it is assumed that the required base station has been deployed in the required scenario. This step may specifically include:

[0070] S10281. Obtain the absolute coordinates of each ground-underground connection point in the second coordinate system. In this embodiment of the present invention, at least four fixed base stations are set around the selected underground environment to receive the pulse radio signals of the points to be located, and a base station is set at each ground-underground connection point to obtain the absolute coordinates (X / Y) of each ground-underground connection point in the second coordinate system (2000 National Geodetic Coordinate System). j ,Y j ,Z j ),j∈(1,2…N) (N is the number of base stations), assuming that the base station with coordinates (X1,Y1,Z1) is the main base station in the GIA underground area.

[0071] S10282. Calculate the distance from the point to be located to the main base station and the angle from the main base station. Suppose the coordinates of each base station are (X j ,Y j ,Z j ), the coordinates of the point to be located Q are (x, y, z), and the formula for calculating the distance from the point to be located to the main base station is:

[0072]

[0073] Where N is the number of base stations, t0 represents the time when the target location to be located transmits the signal, and t j The signal arrival time representing the location of a base station reference point.

[0074] According to the coordinates of the location point Q and the base station assumed in the previous step, it is necessary to measure the time it takes for the UWB signal to reach each base station (X j ,Y j ,Z j ) in the vertical direction (β1, β2…β N ), UWB signal refers to the pulse signal sent by the device to be located, which can be used to calculate the distance from the base station and then locate. The horizontal angle of the node to be located relative to the base station is (θ1, θ2…θ N ).

[0075] S10283. Calculate the coordinates of the point to be located in the second coordinate system according to the distance and angle using the following formula:

[0076]

[0077] Figure 2 A schematic diagram of this angle is shown in FIG. Figure 2 The coordinates of the point to be located, i.e., the point Q to be located, are (x, y, z), and the coordinates of the base station are (X j ,Y j ,Z j ), d is the distance between the point to be located and the base station. According to the above steps, the coordinates of each point to be located can be calculated.

[0078] S10284. Calculate the mark of the point to be located in the time dimension according to the timestamp of the signal sent by the point to be located each time. k (k=1,2,3…), thereby obtaining the mark of the point to be located in the time dimension. The mark of the time dimension can be used to update the position of the point to be located in the second coordinate system in real time. For example, in Amap Navigation, the estimated time to arrive at the destination can be calculated in real time through the movement of the point to be located according to the mark of the point to be located in the time dimension.

[0079] S10285. Obtain the absolute coordinates of the point to be located in a second coordinate system based on the coordinates of the point to be located and the mark of the point to be located in the time dimension.

[0080] According to the coordinates of the points to be located calculated in the previous step, error correction can also be performed. According to the simple iterative least squares method, the errors calculated by each base station are eliminated to obtain the position coordinates (x, y, z) of the points to be located with higher accuracy.

[0081] Through the above steps, the embodiment of the present invention converts the relative coordinates of the position coordinates of each base station obtained from the first coordinate system (Beidou CORS station) into the second coordinate system (2000 National Geodetic Coordinate System). Therefore, the absolute position of the point to be located that changes with time in the second coordinate system (2000 National Geodetic Coordinate System) can be easily calculated from the underground coordinates of the point to be located relative to the base station, thereby establishing a unified above-ground and underground spatial coordinate system (X, Y, Z), thereby achieving seamless connection between above-ground and underground positioning.

[0082] According to some optional embodiments, error calibration can also be performed on the absolute coordinates of the points to be located in the ground and underground connection area in the second coordinate system. According to the above steps, the position coordinates (X j ,Y j ,Z j ), j∈(1,2…N) (N is the number of base stations), by analyzing the coordinate values ​​of the unified point to be located, the error calculated by each base station is eliminated by the least squares method, so as to obtain the position coordinates (X, Y, Z) of the point to be located with higher accuracy.

[0083] The distance D of each coordinate from the respective base station is obtained as [d1, d2…d N ] T , set the weights assigned to N base stations to W = [w1,w2…w N ]. The weight calculated for each base station is w j =1 / d j ,set up

[0084] The coordinates of each base station in the X direction are X = [x1, x2…x N ] T , from which the inertial navigation coordinates under multiple base stations can be calculated:

[0085]

[0086] The same is true for the Y and Z axes. In this way, the errors caused by inertial navigation can be corrected and positioning calibration can be performed based on multiple points to obtain more accurate and reliable coordinates (X, Y, Z).

[0087] S104, performing underground path planning based on the unified coordinate system. After the coordinate system is unified, performing underground path planning may include the following steps:

[0088] S1042. Construct an underground path network based on a unified coordinate system.

[0089] S10421. Abstract the underground facilities into polyhedrons, and obtain the coordinates of the vertices of the polyhedrons in the unified coordinate system. By observing the underground structure, various large underground facilities can be abstracted into polyhedrons, and the coordinates (x, y, z) of the vertices of each polyhedron in the built ground and underground integrated coordinate system can be obtained. The location information of the underground facilities is represented by the polyhedron vertex coordinate set:

[0090] {(x1,y1,z1),(x2,y2,z2),…,(x n ,y n ,z n )}

[0091] Various location-related information is added to the polyhedron. The related information includes, but is not limited to: primary key id, device name, topological map map, facility type type, coordinate set coordinate, general description description, etc., which can be used to (1) provide users with clickable query information in the visual interface and (2) access the coordinate set, device id, and map when searching for the shortest path.

[0092] S10422. Select several points in the polyhedron close to the underground ground area as element points, select several points in the passable area as the first path points, and select several points in the ground area near the ground-underground connection area as the second path points. Select several points in the underground facility polyhedron close to the underground ground area as element points of the underground facility. The i-th element point of polyhedron a is recorded as element[a][i], and the data (x, y, z) is stored. Manually select several points in the passable area as the first path point (x i ,y i ,z i ), denoted as pathpoint[i], it should be ensured that the selected points cover all traversable areas, the accessibility of each selected point, and the measurability of the distance between points in the coordinate system. Several points are also selected in the ground area near the entrance of the underground tunnel as the second path point (x i ,y i ,z i ), denoted as pathpoint_out[i].

[0093] S10423, connect all element points and path points to form a path network topology map. In this step, all element points and path points are reasonably connected. The connection principle is that the distance between two points is less than a certain value, and the connecting line segment between the two points does not pass through the polyhedron to form a road network topology map. The line segment between each point is a passable path. The connection between point i and point j can be expressed as:

[0094]

[0095] If point i and point j are not connected, it can be expressed as:

[0096] path[i][j]=-1.

[0097] S10424. According to the connection between the first path point and the second path point, the path points are divided into connected path points and non-connected path points. According to the connection between the first path point pathpoint[i] and the second path point pathpoint_out[i], the path points are divided into connected path points connect[i] and non-connected path points pathpoint[i]. Through the "connected path points", users can go from the underground topology to the ground topology or vice versa. The "connected path points" must be connected to the path points selected on the ground area, and the path points selected on the ground area must be connected to the "connected path points", that is, for any "connected path point" connect[i], there are one or more ground path points pathpoint_out[j], such that:

[0098] path[connect[i],[pathpoint_out[j]]>=0.

[0099] For the "connected path point", determine the traversable direction, connect[i].in and connect[i].out. When connect[i].in = true, it means that you can go from the ground to the underground through the connected path point, false means you can't. When connect[i].out = true, it means that you can go from the underground to the ground through the connected path point, false means you can't.

[0100] Therefore, the ground road network with fewer restrictions and the underground road network with limited constraints can be connected through the finite interface "connected path point" connect[i], thus achieving the integration of the ground and underground road networks.

[0101] S1044. In the underground path network, for navigation tasks starting from the ground or underground area and ending in the underground or ground area, calculate the shortest path. Suppose there is a point o in the ground area, whose coordinates in the unified ground and underground coordinate system are (x o ,y o ,z o ); a point d in the underground area has coordinates (x d ,y d ,z d ); The path search method from the ground starting point o to the underground end point d is as follows:

[0102] S10441. Obtain a set of polyhedral coordinates of multiple underground facilities.

[0103] S10442, determine whether the end point is located in any polyhedron, if so, lock the first polyhedron where the end point is located; if not, find the path point closest to the midpoint in the first path point array as the end point. Determine the coordinate set of each polyhedron coordinates d (x d ,y d ,z d ) is located in the polyhedron. If so, lock the polyhedron a where the coordinates are located; if not, find the pathpoint [t] closest to point d in the non-connected pathpoint array as the search end point: suppose there are n pathpoints in total, and the distance from the i-th pathpoint to point d is:

[0104]

[0105] S10443. Calculate the shortest distances from the starting point and the end point to the connected path point, and the shortest distance from the starting point to the end point. Calculate the shortest distance from point o to the "connected path point". Traverse the connect array. If the i-th "connected path point" connect[i] satisfies connect[i].in=true, use the A* algorithm to calculate the shortest distance from point o to connect[i], recorded as D(o,connect[i]). Calculate the shortest distance from the "connected path point" to point d. If point d is located in polyhedron a, use the weights of the path segments and use the A* algorithm to calculate the shortest paths D(connect[i],element[a][j]) from all "connected path points" connect[i] to each element point element[a][j] of polyhedron a, and calculate the distance D(element[a][j],d) from each element point element[a][j] of polyhedron a to point d. The shortest distance from the "connected path point" to point d is:

[0106] D(connect[i],d)=min{D(connect[i],element[a][j])+D(element[a][j],d)};

[0107] Otherwise, calculate the shortest path D(connect[i],path point[t]) from all "connected path points" connect[i] to the path point pathpoint[t] closest to point d. The shortest distance from the "connected path point" to point d is:

[0108] D(connect[i],d)=D(connect[i],pathpoint[t])+D(pathpoint[t],d).

[0109] Calculate the shortest distance from point o to point d. The shortest distance from point o to point d is:

[0110] D(o,d)=min{D(o,connect[i])+D(connect[i],element[a][j])}.

[0111] The shortest distances of the three stages are calculated through the above steps S10443 and S10444, and finally the shortest distance from the starting point o to the end point d is obtained.

[0112] S10444. Mark all points passed in the shortest distance from the starting point to the end point and connect them to obtain the shortest path from the starting point to the end point. Mark all points passed in the shortest path D(o,d) from point o to point d and connect them to obtain the shortest path from point o to point d.

[0113] For the case of connecting underground point o to ground point d, the above steps are the same, where the "connected path point" needs to be the point where connect[i].out=true.

[0114] S106: Visualize the planned path. Visualize the planned path includes visualizing the above-ground path and visualizing the underground path.

[0115] Among them, for the visualization of the ground path, when the ground navigation is performed in the outdoor GNSS positioning, the system will confirm the absolute coordinates of the destination in the existing 2000 national geodetic coordinate system, confirm the existing absolute coordinates with the help of the Beidou satellite positioning system and the CORS system, and use the existing map data and relatively mature navigation algorithms to plan the path. The corresponding navigation function and visualization display can be realized by calling the AmapNavi class of existing navigation methods such as Amap.

[0116] The visualization of underground paths can be performed according to the following steps:

[0117] S1061. Determine the method used for underground visualization in an absolute coordinate system according to specific circumstances. When the underground space is relatively regular, the navigation path is abstracted as a set of edges connecting multiple element points. At the same time, the navigable path is determined according to the corresponding value of path[i][j]. Inaccessible paths and areas where pathpoint[i] does not exist are considered inaccessible areas. Under general underground conditions, the height or floor of the user's positioning coordinates is determined by judging the z value of the user's positioning coordinates, and converted into existing two-dimensional navigation.

[0118] S1062. In a scene with a relatively complex environment, the absolute coordinates of the "connected path points" are determined based on the coordinates of multiple base stations, and the corresponding map information is displayed in the form of real scenes and partial three-dimensional modeling. Laser radar is used to scan and obtain laser point cloud data {(P i ,F i )}.

[0119] S1063. To facilitate further processing, the point cloud data is voxelized after registration. The difference between the maximum and minimum values ​​of the point cloud data coordinates in the three directions of XYZ is calculated respectively, and then the length, width and height of the initial voxel are determined according to the three differences. After the calculation is completed, the computer automatically establishes the initial voxel. The established initial voxel already contains all the point cloud data. Then, the Bresenham algorithm is used to remove invalid voxels in the initial voxel. The remaining voxels can form a three-dimensional model of the point cloud data. The point cloud data is projected onto the cubic grid structure {V u,v,w}.

[0120] S1064. Adjust and optimize the cubic grid structure according to actual needs {V u,v,w}. The “connected path points” connect[i] with the access restriction attributes connect[i].in and connect[i].out are spliced ​​and matched. At the same time, the navigation path and the three-dimensional model are superimposed, the minimum distance k of the path relative to the boundary is set, and the route correction parameters are set according to the actual model boundary curvature, so as to obtain the actual navigation path length based on D(o,connect[i]) obtained by the existing shortest path algorithm.

[0121] S1065: Connect the scattered three-dimensional models by using the coordinate information of the path point connect[i] with the access restriction attribute. t ,y t ,z t ), connect the 3D model coordinates with the corresponding positions on the 2D map, and switch to 3D map navigation until the UWB signal disappears within the 3D model. In this way, the independent coordinates in each scan file can be unified with the original map information, and then the map coordinate system can be converted according to the geodetic coordinates of the actual feature points such as pathpoint[i], so as to complete the overall splicing, and finally obtain a complete 3D and 2D combined large map in the same coordinate system.

[0122] S1066. Path[i][j] and pathpoint[i] corresponding to all the areas are superimposed and combined, and the visual navigation function is realized by combining the shortest path calculation method mentioned above.

[0123] The above example illustrates the process of visual expression of underground paths, that is, visual expression is achieved after combining the results of the above steps. Those skilled in the art can also use existing methods to achieve visual expression of paths based on the coordinate systems established in the above steps.

[0124] An embodiment of the present invention further provides a unified above-ground and underground navigation device, comprising:

[0125] Unified coordinate system establishment module, used to establish a unified coordinate system above ground and underground;

[0126] Underground path planning module, used for underground path planning based on a unified coordinate system;

[0127] The visualization expression module is used to visualize the planned path.

[0128] The specific process of each module in the navigation device provided by the above embodiment of the present invention realizing its function is the same as the steps of the navigation method provided by the above embodiment of the present invention, and will not be described one by one here.

[0129] The technical solution of the present invention is described below with a specific example.

[0130] This example uses a mine as the scenario and applies the unified above-ground and underground navigation method provided by the embodiment of the present invention. Seamless navigation can be performed on a mobile phone, and the computer can also monitor the position of people in the tunnel in real time, which is convenient for timely locating the position of trapped people in an emergency situation of mine collapse. Figure 3 The schematic diagram of the integrated above-ground and underground path network in this example is shown in FIG. Figure 3 For detailed explanation, the specific steps are as follows:

[0131] Step 1: Assume that all personnel in the mine carry the navigation system application that can transmit positioning signals. The signal transmitters or mobile phones carried by the workers are used as the positioning points, and each tunnel entrance is set as a base station. For underground areas with less than four base stations, base stations will be installed at the innermost part of the U-shaped tunnel or in the four directions of southeast, northwest, and northeast.

[0132] Step 2: Establish a unified coordinate system.

[0133] First, the present invention assumes that the construction of the second Beidou CORS station is basically completed, and uses the BD network RTK system to obtain the three-dimensional coordinates (X0, Y0, Z0) and speed V of outdoor users all-weather. t and time information.

[0134] The relative coordinates of each tunnel entrance are obtained based on the Beidou CORS station system.

[0135] The Bursa model is used to transform into the 2000 National Geodetic Coordinate System. The transformation formula is:

[0136]

[0137] The relative coordinates of each tunnel entrance are obtained based on the Beidou CORS station system.

[0138] The Bursa model was used to transform the coordinates into the 2000 National Geodetic Coordinate System. The relative coordinates of each tunnel opening were transformed into the absolute coordinates in the 2000 National Geodetic Coordinate System.

[0139] Secondly, the coordinate system is extended to the underground tunnel using inertial navigation technology. In the ground-underground connection area, both ground satellite positioning and underground UWB technology have slight errors, so strapdown inertial navigation technology is used to convert ground-underground navigation. By integrating the acceleration component and angular velocity of the miner's signaler over time, the inertial position change is obtained, and the current inertial position coordinates are obtained based on the previous inertial position and the inertial position change.

[0140] The coordinates of a certain position are obtained by each station through inertial navigation technology, and the errors calculated by each base station are eliminated according to the least squares method, so that the position coordinates (X0, Y0, Z0) of the point to be located with higher accuracy are obtained.

[0141] Finally, the underground positioning in the tunnel uses ultra-wideband technology (UWB) with many advantages such as accurate relative positioning and low power consumption to establish the underground positioning coordinate system. This paper assumes that the required base stations have been deployed underground in this mining area.

[0142] Fixed base stations are set up at each tunnel entrance and underground according to the distance to receive pulse radio signals sent by mine personnel carrying the points to be located.

[0143] The absolute coordinates of each tunnel entrance in the 2000 National Geodetic Coordinate System are obtained by combining the Beidou satellite signal with the CORS system as mentioned above.

[0144] Calculate the distance d between the location of the underground personnel and the base station. The distance between the personnel and each base station is calculated by the time difference between the time when the signal is sent by the signal transmitter and the time when the base station at the tunnel entrance receives the signal. The calculation formula is:

[0145] Calculate the angle between underground personnel and base stations based on the measured vertical pitch angle of the UWB signal reaching each base station and the horizontal angle relative to the base station.

[0146] Calculate the coordinates of the underground personnel. Based on the distance and angle between the required positioning point and each base station obtained in the first two steps, the calculation formula is:

[0147]

[0148] Error analysis is performed. The underground personnel positions obtained by each base station are eliminated according to the simple iterative least square method to eliminate the errors calculated by each base station, thereby obtaining the coordinates of the points to be located with higher accuracy.

[0149] As the point to be located moves, the timestamp sent by the point to be located is obtained each time, thereby obtaining the dynamic positioning of the passenger in the same space-time coordinate system.

[0150] Step 3: Error calibration of the above-ground and underground connection area.

[0151] It is known that in step 2, the position coordinates (X j ,Y j ,Z j),j∈(1,2…N) (N is the number of base stations), analyze the coordinate values ​​of the unified point to be located, and eliminate the errors calculated by each base station through the least squares method, so as to obtain the position coordinates (X, Y, Z) of the point to be located with higher accuracy.

[0152] The distance D of each coordinate from the respective base station is obtained as [d1, d2…d N ] T , set the weights assigned to each N base station to W = [w1,w2…w N ]. The weight calculated for each base station is w j =1 / d j ,set up

[0153] The coordinates of each base station in the X direction are X = [x1, x2…x N ] T , from which the inertial navigation coordinates under multiple base stations can be calculated:

[0154]

[0155] The same is true for the Y and Z axes. In this way, the errors caused by inertial navigation can be corrected and positioning calibration can be performed based on multiple points to obtain more accurate and reliable positioning coordinates (X, Y, Z) of the underground connection area.

[0156] Step 4: Construct an underground path network based on a unified coordinate system.

[0157] First, based on the three-dimensional plan of the mine, the various facilities and equipment in the mine are abstracted into polyhedrons. Based on the absolute coordinates of the integration of the ground and underground obtained in step 2, the coordinate points are connected to form a closed polyhedron, namely equipment a and equipment b. The location information of the equipment under the mine is represented by a set. The coordinates can be used to connect the coordinate points on the topological map to demarcate the area to represent the underground facilities.

[0158] Add various location information for polyhedron equipment a and equipment b, including primary key id, location name, topological map map, facility type type, house number number, coordinate set coordinate, general description description, etc., to create a location information list of spatial objects in the mine.

[0159] The polyhedron display effect is overlaid on the base layer of the topological map in the mine. When the operator reaches or visits the non-exposed space, the underground vector map and the ground navigation system map are connected and displayed.

[0160] Select element[a][1] and element[a][2] at the boundary of the polyhedron of device a close to the ground area as nodes connected to the outside world (generally, the boundary where the device is connected to the road is selected), abstract device a into two element points for representation, and store coordinate data. Similarly, element[b][1] and element[b][2] are selected for device b. Manually select multiple path points in the passable area, abstract the passable road into edges connected by multiple nodes, and the selected points should ensure the traversability, accessibility, and measurability of the road network model. Manually select several path points in the ground area near the entrance of the underground tunnel.

[0161] Reasonably connect all element points and path points to form a road network topology map, and the line segments between the points are the passable paths.

[0162] According to the actual connectivity between the path points and the ground, the path points are divided into connected path points and non-connected path points. There are two "connected path points", connect[1] and connect[2]. Record whether connect[1] and connect[2] are allowed from underground to above ground or from above ground to underground.

[0163] In this way, the ground road network of the mine with less restrictions and the underground road network with limited constraints can be connected through the limited interface "connecting path points", so as to achieve the integration of the ground and underground road networks.

[0164] Step 5: Underground shortest path algorithm based on unified coordinates

[0165] Assume there is a point o outside the mine and a point d inside the mine, and obtain the coordinates of the two points in the integrated coordinate system of the ground and underground. The path search method from point o to point d is as follows:

[0166] Point d is inside the polyhedron device b and locks the polyhedron device b.

[0167] Calculate the shortest distance from point o to connect[1] and connect[2] respectively. If connect[i] satisfies the requirement of being able to go from above ground to underground, use the A* algorithm to calculate the shortest distance from point o to connect[i].

[0168] Calculate the shortest distance from the "connected path point" to point d. Using the weights of the path segments, use the A* algorithm to calculate the shortest paths D(connect[1],element[b][1]), D(connect[1],element[b][2]), d(connect[2],element[b][1]), and D(connect[2],element[b][2]) from connect[1] and connect[2] to each element point element[b][1] and element[b][2] of polyhedron device b, and calculate the distances D(element[b][1],d) and D(element[b][2],d) from element[b][1] and element[b][2] to point d, and obtain the shortest distances D(connect[1],d) and D(connect[2],d) from each "connected path point" to point d.

[0169] Calculate the shortest distance from point o to point d. The shortest path from point o to point d is the minimum value of the distance from point o to the "connected path point" and the distance from the same "connected path point" to point d. Compare and get the shortest distance from point o to point d D(o,d).

[0170] Mark all the points passed in the shortest path from point o to point d, connect them, and display them on the user interface to obtain the shortest path from point o to point d.

[0171] Similarly, from point d underground to point o above ground, the "connected path point" needs to select a point that allows going from underground to above ground.

[0172] Step 6: Path Visualization

[0173] First, when the outdoor GNSS positioning is in place and the outdoor navigation is being performed, the system will confirm the absolute coordinates of the destination in the existing 2000 national geodetic coordinate system, and with the help of the Beidou satellite positioning system and the CORS system to confirm the existing absolute coordinates, the existing map data and relatively mature navigation algorithms can be used for path planning. The outdoor part of this article can implement the corresponding navigation function and visual display by calling the AmapNavi class of the AutoNavi map. Secondly, the method used for visual navigation in the absolute coordinate system is confirmed in the mine tunnel according to the specific situation.

[0174] By determining the z value, i.e. the height value, of the passenger's location coordinates, the depth corresponding to the lane is determined and converted into the existing two-dimensional navigation.

[0175] In scenes with more complex environments, such as bifurcated lanes, interlaced up and down, and with more equipment and terrain, the absolute coordinates of the "connected path points" are determined based on the coordinates of the deployed base stations, and the corresponding map information is displayed in the form of real scenes and partial 3D modeling. The 3D modeling part adopts the laser scanning modeling method. After obtaining the corresponding laser point cloud data, the point cloud data is simplified by grid processing to obtain a preliminary 3D model.

[0176] Adjust and optimize the size of the cubic grid structure according to actual needs. For example, if the scene route is more complex, reduce the cubic grid size. If the scene has a high degree of repetition and is more regular, appropriately enlarge the cubic grid size, so as to further adjust the model refinement and data volume.

[0177] After confirming the map location information based on the relative location information of each path point, the map data is spliced ​​and matched and converted into the same coordinate system, thereby obtaining a unified local coordinate system.

[0178] Then, the local coordinate system is converted according to the feature point coordinate information and the geodetic coordinate information of the UWB base station confirmed by the Beidou CORS system to complete the docking with the coordinate systems of other parts of the map, thereby realizing the integration of the coordinate system.

[0179] Afterwards, taking into account the operation of regional facilities and the lag of map information, the path previously taken by actual navigation users, the navigation path obtained by the existing shortest path algorithm, and the actual feasible path based on preset limited conditions are compared and optimized to obtain the final navigation path.

[0180] Finally, all the map information is combined and connected. The path points with access restriction attributes corresponding to all the areas are superimposed and combined, and combined with the navigation method provided by the above embodiment of the present invention, the navigation function is finally realized.

[0181] In summary, the embodiment of the present invention relates to a unified navigation method and device for the ground and underground, the method comprising the steps of: establishing a unified coordinate system for the ground and underground; planning an underground path based on the unified coordinate system; and visually expressing the planned path. The technical solution of the embodiment of the present invention establishes an absolute coordinate system by integrating and converting the positioning of different coordinate systems into the 2000 National Geodetic Coordinate System. Only one available angle vector is required for more accurate positioning, and the availability is higher under special conditions. At the same time, the amount of calculation except for the start of positioning and the participation of new system signals in positioning is reduced, and the power consumption of the equipment is reduced. When a new system is added, it is also easier to further convert the relative coordinates of the original system into absolute coordinates based on the existing positioning, thereby realizing seamless switching between positioning systems. For special areas where external signals cannot be received and base stations cannot be configured, the absolute coordinates can be determined through image information, NFC points to be positioned, etc., so as to further calibrate and optimize the terminal positioning.

[0182] It should be understood that the discussion of any of the above embodiments is only exemplary and is not intended to imply that the scope of the present invention (including claims) is limited to these examples; under the idea of ​​the present invention, the technical features in the above embodiments or different embodiments may also be combined, the steps may be implemented in any order, and there are many other changes in different aspects of one or more embodiments of the present invention as described above, which are not provided in detail for the sake of simplicity. The above specific embodiments of the present invention are only used to illustrate or explain the principles of the present invention, and do not constitute a limitation of the present invention. Therefore, any modifications, equivalent substitutions, improvements, etc. made without departing from the spirit and scope of the present invention should be included in the scope of protection of the present invention. In addition, the claims attached to the present invention are intended to cover all changes and modifications that fall within the scope and boundaries of the attached claims, or the equivalent forms of such scope and boundaries.

Claims

1. A unified ground and underground navigation method, characterized in that: Includes steps: Establish a unified coordinate system above and below ground; The underground path planning is performed based on the unified coordinate system, including: constructing an underground path network based on the unified coordinate system; wherein the constructing the underground path network includes: Abstracting the underground facility into a polyhedron, and obtaining coordinates of the vertices of the polyhedron in the unified coordinate system; Select several points in the ground area close to the underground of the polyhedron as element points, select several points in the passable area as first path points, and select several points in the ground area near the ground-underground junction area as second path points; Connect all element points and path points to form a path network topology graph; wherein the connection principle for forming the path network topology graph is that the distance between two points is less than a certain value, and the connecting line segment between the two points does not pass through the polyhedron; According to the connection between the first path point and the second path point, the path points are divided into connected path points and disconnected path points; Visualize the planned path.

2. The method according to claim 1, characterized in that Establishing a unified coordinate system above and below ground includes: Obtain the three-dimensional coordinates of the outdoor first coordinate system; Converting the outdoor first coordinate system into a second coordinate system, where the second coordinate system is an absolute coordinate system; Calculate the absolute coordinates of the ground and underground connection area and the points to be located in the second coordinate system; Calculate the absolute coordinates of the point to be located in the underground area in the second coordinate system.

3. The method according to claim 2, characterized in that The absolute coordinates of the points to be located in the second coordinate system for calculating the above-ground and underground connection areas include: Calculate the inertial position change of the point to be positioned through the acceleration and angular velocity of the point to be positioned; The absolute coordinates of the point to be located at the second coordinate are obtained according to the previous inertial position coordinates and the inertial position change of the point to be located.

4. The method according to claim 3, characterized in that The calculation of the absolute coordinates of the point to be located in the underground area in the second coordinate system includes: Obtain the absolute coordinates of each ground and underground connection point in the second coordinate system; Calculate the distance from the point to be located to the main base station and the angle from the main base station; Calculate the coordinates of the point to be located in the second coordinate system according to the distance and angle; Calculate the mark of the point to be located in the time dimension according to the timestamp of the signal sent each time the point to be located moves; The absolute coordinates of the point to be located in the second coordinate system are obtained based on the coordinates of the point to be located and the mark of the point to be located in the time dimension.

5. The method according to claim 2, characterized in that: The establishment of a unified above-ground and underground coordinate system further includes: Error calibration is performed on the absolute coordinates of the points to be positioned in the ground and underground connection area in the second coordinate system.

6. The method according to claim 1, characterized in that The underground path planning based on the unified coordinate system also includes: In an underground path network, the shortest path is calculated for navigation tasks that start in an above-ground or underground area and end in an underground or above-ground area.

7. The method according to claim 6, characterized in that The calculation of the shortest path comprises: Obtaining a polyhedral coordinate set of multiple underground facilities; Determine whether the end point is located in any polyhedron. If so, lock the first polyhedron where the end point is located; if not, find the path point closest to the midpoint in the first path point array as the end point; Calculate the shortest distances from the starting point and the end point to the connected path points, as well as the shortest distance from the starting point to the end point; Mark all the points passed in the shortest distance from the starting point to the end point and connect them to get the shortest path from the starting point to the end point.

8. A unified navigation device for above-ground and underground, characterized in that: include: Unified coordinate system establishment module, used to establish a unified coordinate system above ground and underground; The underground path planning module is used to perform underground path planning based on a unified coordinate system, including: constructing an underground path network based on the unified coordinate system; wherein, constructing the underground path network includes: Abstracting the underground facility into a polyhedron, and obtaining coordinates of the vertices of the polyhedron in the unified coordinate system; Select several points in the ground area close to the underground of the polyhedron as element points, select several points in the passable area as first path points, and select several points in the ground area near the ground-underground junction area as second path points; Connect all element points and path points to form a path network topology graph; wherein the connection principle for forming the path network topology graph is that the distance between two points is less than a certain value, and the connecting line segment between the two points does not pass through the polyhedron; According to the connection between the first path point and the second path point, the path points are divided into connected path points and disconnected path points; The visualization module is used to visualize the planned path.

Images