Laser radar assisted indoor wireless sensor node self-positioning method and device
Through the lidar-assisted method, point cloud maps and distance measurements are used to construct cost functions to self-position the wireless sensor nodes, solving the problems of low indoor positioning accuracy and difficult coordinate mapping, and achieving high-precision self-positioning and network map matching .
Patent Information
- Application Number
- CN202411212290.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-30
- Publication Date
- 2024-11-12
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In indoor positioning technology, existing wireless sensor nodes have low accuracy due to multipath and interference, and it is difficult to realize coordinate mapping of the physical world without external reference conditions.
Using the lidar-assisted method, the distance constraint and spatial constraint cost functions are constructed through the distance measurement between wireless sensor nodes and the point cloud map acquired by lidar, and the cost functions of the nodes are optimized by combining the plane fitting of the local map and the finite circular region approximation. Self-positioning results.
It improves the self-positioning accuracy of wireless sensor nodes, realizes more accurate mapping of relative coordinates and physical world, and enhances the matching and fusion of sensor networks and lidar point cloud maps.
Smart Images

Figure CN118936457A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of wireless positioning technology, and in particular to a laser radar-assisted indoor wireless sensor node self-positioning method and a laser radar-assisted indoor wireless sensor node self-positioning device. Background Art
[0002] With the development of radio sensor technology, wireless sensor network (WSN) positioning technology is receiving more and more attention. In indoor environments, since the Global Navigation Satellite System (GNSS) signal is unavailable due to the obstruction of walls, WSN-based positioning solutions have been widely used to provide positioning services for users in indoor environments.
[0003] In the existing indoor positioning technology of wireless sensor nodes, positioning is mainly achieved by measuring the signal characteristics of the current position of the node or the geometric relationship such as the distance or angle between the node and other nodes. In these technologies, it is often necessary to measure the signal characteristics of each position in advance or to map the actual position of the reference node in advance. However, this time-consuming preparation work may greatly affect the flexibility of the positioning system and limit its application scenarios. For example, in caves that are difficult to reach manually or sudden disaster relief scenarios, it is often difficult to complete complex measurement and mapping work in advance. Therefore, many existing technologies focus on the self-positioning technology of sensor nodes without external reference benchmark conditions. For example, through radio ranging technology, specifically through the signal transmission and reception between nodes, the mutual distance between nodes is achieved, so as to calculate the relative position of each node and realize the autonomous positioning of WSN without pre-deployment conditions.
[0004] There are two main problems with the existing wireless sensor node self-positioning technology. One of the problems is that in indoor environments, due to the influence of multipath and interference, the positioning accuracy is not high enough and the positioning error is large. The other problem (i.e., the reference alignment problem) is that WSN only obtains a set of relative coordinates through mutual measurement of nodes without external reference conditions, which makes it difficult to map the coordinate values to the physical world. Summary of the invention
[0005] In view of this, the present application proposes a laser radar-assisted indoor wireless sensor node self-positioning method and a laser radar-assisted indoor wireless sensor node self-positioning device to at least solve the above-mentioned existing technical problems.
[0006] According to an embodiment of the present application, a laser radar-assisted indoor wireless sensor node self-positioning method can be used to assist multiple wireless sensor nodes to self-position indoors by laser radar, wherein the indoors contains at least one object, and the multiple wireless sensor nodes are arranged on the surface of the at least one object. The laser radar-assisted indoor wireless sensor node self-positioning method may include:
[0007] A distance measurement value set can be obtained by measuring the distance between multiple wireless sensor nodes, and a point cloud map of the room can be obtained by a laser radar;
[0008] A preliminary self-positioning result of the plurality of wireless sensor nodes in the point cloud map may be obtained according to the distance measurement value set and the point cloud map;
[0009] The distance constraint cost function of the corresponding wireless sensor node can be constructed based on the point cloud map and the distance measurement value set;
[0010] In the point cloud map, the wireless sensor node can be taken as the sphere center and the sphere range can be selected with a specific radius as the local map of the corresponding wireless sensor node;
[0011] The spatial constraint cost function of the corresponding wireless sensor node can be constructed in the local map by plane fitting and finite circular area approximation;
[0012] A complete cost function can be constructed based on the space constraint cost function and the distance constraint cost function; and
[0013] Taking the preliminary self-positioning result as the initial value, the value of the function variable that makes the complete cost function achieve the minimum value is obtained, and the value is used as the accurate self-positioning result of the wireless sensor node.
[0014] In the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application,
[0015] The node relative coordinates of the plurality of wireless sensor nodes may be obtained according to the distance measurement value set; and
[0016] The relative coordinates of the multiple wireless sensor nodes may be preliminarily matched with the point cloud map to obtain self-positioning results of the multiple wireless sensor nodes in the point cloud map.
[0017] In the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application, the spatial constraint cost function can be constructed as:
[0018]
[0019] Among them, h′(x) represents the spatial constraint cost function,
[0020] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0021] n represents a wireless sensor node, n∈N,
[0022] x n represents the coordinates of wireless sensor node n,
[0023] represents the homogeneous coordinates of the wireless sensor node n,
[0024] P n represents a set of multiple planes to which any wireless sensor node n is attached obtained by plane fitting in the local map,
[0025] represents the plane representation parameter after normalizing the multiple planes,
[0027] Wherein, in the point cloud map, the x of the wireless sensor node n is selected n is the center of the sphere and the radius is R n is the sphere with radius as the local map, M n represents the local map, M represents the point cloud map, where
[0028] In the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application, the spatial constraint cost function can be further constructed as:
[0029]
[0030] Among them, h(X) represents the spatial constraint cost function,
[0031] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0032] n represents a wireless sensor node, n∈N,
[0033] x n represents the coordinates of wireless sensor node n,
[0034] represents the homogeneous coordinates of the wireless sensor node n,
[0035] P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map.
[0036] represents the plane representation parameter after normalizing the multiple planes,
[0037] represents the center of the fitted circular area,
[0038] r n,p represents the center of the fitted circular plane,
[0039] Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located,
[0040] Represents wireless sensor node x n The projection and center of the circle on the plane where the fitted circular area is located The distance
[0041] A represents a large numerical coefficient,
[0042] L is an operator, which means L(x)=max(0,x).
[0043] In the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application, the distance cost function can be constructed as:
[0044]
[0045] Among them, x n represents the nth wireless sensor node,
[0046] x i represents the i-th wireless sensor node,
[0047] Represents the distance between the nth wireless sensor node and the i-th wireless sensor node.
[0048] In the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application, the complete cost function can be constructed as:
[0049]
[0050] Among them, f(x) represents the complete cost function,
[0051] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0052] n represents a wireless sensor node, n∈N,
[0053] x n represents the coordinates of wireless sensor node n,
[0054] represents the homogeneous coordinates of the wireless sensor node n,
[0055] P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map.
[0056] represents the plane representation parameter after normalizing the multiple planes,
[0057] x n represents the wireless sensor node whose belonging plane is to be determined,
[0058] represents the center of the fitted circular area,
[0059] r n,p represents the center of the fitted circular plane,
[0060] Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located,
[0061] Represents point x n The projection and center of the circle on the plane where the fitted circular area is located The distance
[0062] A represents a large numerical coefficient,
[0063] L means L(x)=max(0,x),
[0064] x i represents the i-th wireless sensor node,
[0065] represents the distance between the nth wireless sensor node and the ith wireless sensor node,
[0066] σ R Indicates the ranging accuracy,
[0067] σ M Indicates the point cloud map accuracy.
[0068] In the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application, obtaining the value of the variable that minimizes the complete cost function includes:
[0069] For each node n, the local cost function f can be obtained n (X) Get the local minimum x n , repeat the above steps until the coordinates x of each node are n Converge so that the complete cost function f(X) reaches the minimum value, where:
[0070]
[0071] According to an embodiment of the present application, a laser radar-assisted indoor wireless sensor node self-positioning device can be used to assist multiple wireless sensor nodes to perform self-positioning indoors by laser radar, wherein the indoors contains at least one object, and the multiple wireless sensor nodes are arranged on the surface of the at least one object. The laser radar-assisted indoor wireless sensor node self-positioning device may include:
[0072] The data acquisition module may be configured to obtain a distance measurement value set by measuring the distance between the multiple wireless sensor nodes, and to obtain a point cloud map of the room by a laser radar;
[0073] The preliminary positioning module may be configured to obtain preliminary self-positioning results of the plurality of wireless sensor nodes in the point cloud map according to the distance measurement value set and the point cloud map;
[0074] A first construction module may be configured to construct a distance constraint cost function of a corresponding wireless sensor node based on the point cloud map and the distance measurement value set;
[0075] The local map selection module may be configured to select a sphere range with a specific radius with the wireless sensor node as the sphere center in the point cloud map as the local map of the corresponding wireless sensor node;
[0076] The second construction module may be configured to construct a spatial constraint cost function of the corresponding wireless sensor node in the local map by plane fitting and finite circular area approximation;
[0077] A third building module may be configured to build a complete cost function based on the space constraint cost function and the distance constraint cost function; and
[0078] The precise positioning module can be configured to use the preliminary self-positioning result as the initial value, obtain the value of the function variable that makes the complete cost function obtain the minimum value, and use it as the precise self-positioning result of the wireless sensor node.
[0079] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the preliminary positioning module can be configured as follows:
[0080] Obtaining node relative coordinates of the plurality of wireless sensor nodes according to the distance measurement value set; and
[0081] The relative coordinates of the multiple wireless sensor nodes are preliminarily matched with the point cloud map to obtain the self-positioning results of the multiple wireless sensor nodes in the point cloud map.
[0082] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the spatial constraint cost function can be constructed as:
[0083]
[0084] Among them, h′(X) represents the spatial constraint cost function,
[0085] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0086] n represents a wireless sensor node, n∈N,
[0087] x n represents the coordinates of wireless sensor node n,
[0088] represents the homogeneous coordinates of the wireless sensor node n,
[0089] P n represents a set of multiple planes to which any wireless sensor node n is attached obtained by plane fitting in the local map,
[0090] represents the plane representation parameter after normalizing the multiple planes,
[0091] Wherein, in the point cloud map, the x of the wireless sensor node n is selected n is the center of the sphere and the radius is R n is the sphere with radius as the local map, M n represents the local map, M represents the point cloud map, where
[0092] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the spatial constraint cost function can be further constructed as:
[0093]
[0094] Among them, h(X) represents the spatial constraint cost function,
[0095] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0096] n represents a wireless sensor node, n∈N,
[0097] x n represents the coordinates of wireless sensor node n,
[0098] represents the homogeneous coordinates of the wireless sensor node n,
[0099] P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map.
[0100] represents the plane representation parameter after normalizing the multiple planes,
[0101]
[0102] represents the center of the fitted circular area,
[0103] r n,p represents the center of the fitted circular plane,
[0104] Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located,
[0105] Represents wireless sensor node x n The projection and center of the circle on the plane where the fitted circular area is located The distance
[0106] A represents a large numerical coefficient,
[0107] L is an operator, which means L(x)=max(0,x).
[0108] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the distance cost function can be constructed as:
[0109]
[0110] Among them, x n represents the nth wireless sensor node,
[0111] x i represents the i-th wireless sensor node,
[0112] Represents the distance between the nth wireless sensor node and the i-th wireless sensor node.
[0113] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the complete cost function can be constructed as:
[0114]
[0115] Among them, f(X) represents the complete cost function,
[0116] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0117] n represents a wireless sensor node, n∈N,
[0118] x n represents the coordinates of wireless sensor node n,
[0119] represents the homogeneous coordinates of the wireless sensor node n,
[0120] P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map.
[0121] represents the plane representation parameter after normalizing the multiple planes,
[0122]
[0123] x n represents the wireless sensor node whose belonging plane is to be determined,
[0124] represents the center of the fitted circular area,
[0125] r n,p represents the center of the fitted circular plane,
[0126] Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located,
[0127] Represents point x n The projection and center of the circle on the plane where the fitted circular area is located The distance
[0128] A represents a large numerical coefficient,
[0129] L means L(x)=max(0,x),
[0130] x i represents the i-th wireless sensor node,
[0131] represents the distance between the nth wireless sensor node and the ith wireless sensor node,
[0132] σ R Indicates the ranging accuracy,
[0133] σ M Indicates the point cloud map accuracy.
[0134] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the precise positioning module can be configured as follows:
[0135] For each node n, obtain the local cost function f n (X) Get the local minimum x n , repeat the above steps until the coordinates x of each node are n Converge so that the complete cost function f(X) reaches the minimum value, where:
[0136]
[0137] According to the laser radar-assisted indoor wireless sensor node self-positioning method and laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiments of the present invention, specifically, by modeling the point cloud map obtained by the laser radar, combined with the fact that the nodes are usually arranged on the surface of the object, the local point cloud map is additionally introduced to provide spatial constraints for each node of the wireless sensor. By approximating the plane fitting of the local point cloud map with the limited area of the object surface, the spatial constraints are combined with the distance constraints provided by the radio ranging results between the nodes, and the self-positioning results of each node under the joint constraints are solved. Through the technical solution according to the present invention, it is possible to effectively combine the spatial constraints provided by the laser radar point cloud map, further improve the self-positioning accuracy of the sensor node, and further optimize the relative position relationship between each node and the map environment, so as to achieve better matching and fusion of the sensor network and the real-time laser radar point cloud map. BRIEF DESCRIPTION OF THE DRAWINGS
[0138] For purposes of illustration and not limitation, the present invention will now be described in terms of preferred embodiments thereof, with particular reference to the accompanying drawings, in which:
[0139] Figure 1 A schematic diagram of a laser radar-assisted indoor wireless sensor node self-positioning method according to an embodiment of the present invention;
[0140] Figure 2A schematic diagram of plane fitting and finite circular area approximation for a wireless sensor node according to an embodiment of the present invention; and
[0141] Figure 3 Schematic diagram of a device for self-positioning of indoor wireless sensor nodes assisted by a laser radar according to an embodiment of the present invention. DETAILED DESCRIPTION
[0142] The technical solution of the present application will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described implementation is only a part of the implementation of the present application, but not all of it. Based on the implementation in the present application, all other implementations obtained by ordinary technicians in the field without creative work are within the scope of the patent protection intended by the present application.
[0143] This application proposes a laser radar assisted indoor wireless sensor node self-positioning method, which is used to assist multiple wireless sensor nodes to self-position indoors with laser radar, wherein the indoors contain at least one object, and multiple wireless sensor nodes are arranged on the surface of at least one object. The specific positions of multiple wireless sensor nodes can be flexibly set, but at least some of the nodes must meet mutual visibility. Figure 1 As shown, the laser radar-assisted indoor wireless sensor node self-positioning method according to the embodiment of the present application includes the following steps A to G:
[0144] Step A: A distance measurement value set can be obtained by measuring the distance between multiple wireless sensor nodes, and a point cloud map of the room can be obtained by a laser radar. In step A, according to a preferred embodiment of the present application, the number of wireless sensor nodes is not less than 6, and / or the number of connections between each node and other nodes is not less than 4, and / or the angular resolution of the laser radar is not less than 0.4°, and / or the point cloud density of the laser radar is not less than 200,000 per second.
[0145] Specifically, in an indoor scene, N wireless sensor nodes that can measure distance between each other are deployed, and a laser radar is also set up. N wireless sensor nodes that can measure distance between each other constitute a node set N = {1, ..., N}, and the coordinate set of the wireless sensor nodes is The coordinate of the nth node is x n =(x n ,y n ,z n ) T , n=1…N. Through radio ranging technology, we can get the distance measurement value set D between any two nodes. The distance measurement value between node i and node j can be Modeled as: n Rrepresents the noise of the distance measurement, which can be modeled as independent and identically distributed Gaussian noise. e ij Use 1 or 0 to indicate whether the distance between two nodes is available.
[0146] In addition, after the laser radar scan is completed and the indoor scene is mapped in real time, a point cloud map M can be obtained with an accuracy of σ M The complete point cloud map M is often very large in size, and the object surfaces in the map are much larger than the number of actual node layouts or surfaces that need to be considered in the calculation. If all point clouds in the map are directly modeled and jointly solved on this basis, the computational load may be too large. At the same time, affected by spatial similarity, the convergence may yield unexpected and unreasonable results.
[0147] Step B: The preliminary self-positioning results of the multiple wireless sensor nodes in the point cloud map can be obtained based on the distance measurement value set and the point cloud map. In step B, according to one embodiment of the present application, the node relative coordinates of the multiple wireless sensor nodes can be obtained based on the distance measurement value set; and the relative coordinates of the multiple wireless sensor nodes are preliminarily matched with the point cloud map to obtain the self-positioning results of the multiple wireless sensor nodes in the point cloud map. Among them, any technical solution in the prior art can be used as a method for preliminary coordinate system matching, for example, a method that uses sampling point pairs in a trajectory and calculates the conversion relationship between the two coordinate systems through a trajectory matching process.
[0148] According to an embodiment of the present application, the relative coordinates of the nodes can be solved according to the distance measurement value set D. The point cloud map M obtained by the operation of the laser radar is essentially composed of a set of three-dimensional points X M Composition, coordinates are On this basis, after preliminary coordinate system matching, the preliminary self-positioning result X0 of each sensor node in the point cloud map can be obtained. The preliminary self-positioning result X0 is the result of the sensor node completing the positioning and preliminary matching with the point cloud map. Although it may not be accurate enough, it can still provide a rough reference for the actual layout of the node. X0 will be used as the initial value for further optimization of the self-positioning result.
[0149] Step C: Construct the distance constraint cost function of the corresponding wireless sensor node based on the point cloud map and the distance measurement value set. The distance measurement value set D provides a distance constraint for solving the node coordinates, which can be recorded as g(X),
[0150] Specifically,
[0151]
[0152] Among them, x nrepresents the nth wireless sensor node, x i represents the i-th wireless sensor node, Indicates the distance between the nth wireless sensor node and the ith wireless sensor node. In g(X), the weight of each distance pair is normalized using the distance measurement value based on the s-stress criterion. The distance constraint g(X,D) is a nonlinear and non-convex constraint. Since it is difficult to jointly solve it, it is necessary to further introduce a spatial constraint.
[0153] Step D: In the point cloud map, the wireless sensor node can be used as the center of the sphere and the sphere range can be selected with a specific radius as the local map of the corresponding wireless sensor node. Since the sensor node is directly set on the surface of the object, that is, the sensor node should be within the range of a certain plane composed of the point cloud. Therefore, introducing the point cloud map M as a spatial constraint into the node self-positioning problem can provide additional environmental information for the node coordinate solution, and by constraining the range of each node solution, it can further improve the positioning accuracy while converging the node position to a map position that is more in line with the actual layout.
[0154] According to an embodiment of the present application, in order to realize the extraction of local map from local point cloud, a spherical point cloud can be selected. Specifically, first, for any node n∈N in the point cloud map M, its coordinate x n is the center of the sphere, R n As the radius, extract all point clouds within the spherical range as a local map To process. Local map M n Each plane in is a possible plane to which node n belongs, and in subsequent calculations, the most suitable plane needs to be selected based on the measurement and the positions of other nodes. n In the selection of , different radii are selected according to the possible positioning error range of each node. For nodes with large positioning errors, it is necessary to extract a larger local map to ensure that the actual belonging plane of the node can be included in the local map; conversely, for nodes with higher accuracy, the corresponding local map range is smaller to reduce the computational complexity and the influence of spatial similarity. Specifically, in addition to the influence of the algorithm, the accuracy of the self-positioning algorithm based on inter-node ranging is mainly affected by the ranging accuracy and the geometric distribution of the nodes. Considering the influence of ranging accuracy, the ranging error can be modeled as a standard deviation of σ R Considering the influence of node geometric distribution, we can analyze it by the geometric dilution of precision (GDOP) of each node. For a node network with coordinates X, the GDOP value of node n is denoted as GDOP n , the calculation formula is as follows:
[0155]
[0156]
[0157] where tr(·) represents the trace of the matrix.
[0158] On this basis, the accuracy σ of node n n It can be roughly estimated as:
[0159] σ n =σ R ×GDOP n .
[0160] Finally, according to the 3-σ criterion, the local map radius R is selected n For R n =3×σ n .
[0161] At this point, for any node n, a local map M of appropriate size is extracted from the global map M n , for subsequent processing and modeling.
[0162] Step E constructs the spatial constraint cost function of the corresponding wireless sensor node in the local map by plane fitting and finite circular area approximation. n Afterwards, the planes to which the nodes in the point cloud can be attached are modeled by fitting.
[0163] In one embodiment of the present application, multiple planes are extracted from the point cloud, and the representation parameters ρ of each plane are fitted, and for any point (x, y, z) on the plane T , satisfying (x, y, z, 1)·ρ=0. After the above plane fitting process, for the local map M n , we can get P n planes that may be attached to node n, which constitute the set P n and its normalized plane representation parameters On this basis, node n to plane Distance It can be expressed as in is the homogeneous coordinate of node n. At this time, the node is attached to a plane in the local map, which provides spatial constraints. We can initially use the least squares idea to model it as a cost function h′(X).
[0164] Specifically,
[0165]
[0166] Among them, h′(X) represents the spatial constraint cost function,
[0167] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0168] n represents a wireless sensor node, n∈N,
[0169] x n represents the coordinates of wireless sensor node n,
[0170] represents the homogeneous coordinates of the wireless sensor node n,
[0171] P n represents a set of multiple planes to which any wireless sensor node n is attached obtained by plane fitting in the local map,
[0172] represents the plane representation parameter after normalizing the multiple planes,
[0173]
[0174] Using only plane parameters as spatial constraints may be sufficient for empty indoor scenes with only the ground and walls, because at this time the point clouds of these planes in the local map are often distributed around the node, but if the scene includes small planar objects such as tables and chairs, the node may determine that it belongs to the plane provided by these objects but does not converge to the surface of the actual object, or it may be affected by the wrong belonging plane judgment and this problem of multiple plane ambiguity will be more serious in actual complex indoor scenes. Therefore, on the basis of obtaining the plane parameters, it is necessary to further constrain and model the actual range of the surface of the object in the plane. For example, in a specific embodiment of the present application, a circular area can be selected to approximate the range of the surface of the object in the plane. Specifically, two circular areas in the plane obtained by fitting are used to approximate the surface of the object represented by two groups of point clouds, respectively, to provide more accurate boundaries and ranges for spatial constraints. After approximation, the two areas can be clearly distinguished relative to the node position, thereby reducing the ambiguity of the spatial constraints provided by the point cloud.
[0175] In addition, for the center of the circular area and radius r n,p When performing plane fitting, the local map is used to calculate the plane parameters. Point Cloud The point cloud coordinates are Select The center of mass is taken as the center of the circle Take can include The length of all point clouds in is radius r n,p ,Right now:
[0176]
[0177]
[0178] The concept of "nodes on the surface of an object" can be approximately modeled by combining two conditions: (1) the distance from the node to the plane is zero, and (2) the projection of the node on the plane lies within a circular region. Figure 2 , we can understand that for node n and the plane it may be attached to We can get node n on the plane The projection point and the center of the circle Distance satisfy:
[0179]
[0180] On this basis, the above conditions can be modeled as:
[0181]
[0182] When designing the optimal spatial constraint cost function, the node may appear at any position on the surface of the object, so the nodes in the circular area should not be affected by the range constraint. In addition, if the node is not on the surface of the object, the node should be pulled into the object range and the minimum cost should be selected instead of simply excluding the node. Therefore, the derivative of the cost function should not be 0 when the node is out of range, which will make the range constraint unable to play a role in updating the coordinates. Finally, the spatial cost function should be globally differentiable so that it can be solved by optimization tools later. Combining these conditions, the spatial constraint cost function can be h(X),
[0183] Specifically,
[0184]
[0185]
[0186] The meaning of the parameters of this formula has been explained above and will not be repeated here. However, it should be noted that A is a coefficient with a larger value (for example, a value greater than 1000), which represents the penalty for nodes exceeding the circular area, so that the final result converges to a circular area approximating the surface of the object.
[0187] For L(·), we use the concept of activation function in neural networks to select the form of ReLU function, that is, L(x) = max(0,x). L(·) makes the node in the circular area, that is, When , it will not be affected by the circular range, and only needs to be close to the plane to meet the spatial constraints; when the node is outside the circular area, that is, When , it has a more obvious gradient, which can make the solution of the node outside the circular area converge quickly to the inside of the area.
[0188] Step F: A complete cost function may be constructed based on the space constraint cost function and the distance constraint cost function.
[0189] According to one embodiment of the present application, the spatial constraint cost function and the distance constraint cost function can be added to obtain a complete cost function, that is, f(X)=h(X)+g(X). The calculation formula of h(X) is detailed in step E, and the calculation formula of g(X) is detailed in step C. Adding the two together, the following calculation formula can be obtained.
[0190]
[0191] Step G: The preliminary self-positioning result can be used as the initial value to obtain the value of the function variable that minimizes the complete cost function as the accurate self-positioning result of the wireless sensor node.
[0192] Step G aims to solve Here, argmin means "the goal is to minimize the value of a function". Specifically, the initial self-positioning result X0 obtained in step B can be used as the initial value. In each iteration, it is assumed that the other variables are fixed and only one variable is used to find the extreme value under the current conditions, and each variable is solved in turn. In this way, a complex high-dimensional optimization problem can be decomposed into multiple simple one-dimensional optimization problems, thereby reducing the complexity of the solution.
[0193] According to an embodiment of the present application, obtaining the value of the variable that makes the complete cost function obtain the minimum value may include: for each node n, obtaining the value that makes the local cost function f n (X) Get the local minimum x n , repeat the above steps until the coordinates x of each node are n Converge so that the complete cost function f(X) reaches the minimum value, and the node coordinates x of each node that makes the global cost function f(X) reach the minimum value n The value of is the accurate self-positioning result of the wireless sensor node, where
[0194]
[0195] The present application also proposes a laser radar assisted indoor wireless sensor node self-positioning device, which is used to assist multiple wireless sensor nodes to perform self-positioning indoors by using a laser radar, wherein the indoors contain at least one object, and multiple wireless sensor nodes are arranged on the surface of the at least one object. Figure 3 As shown, the laser radar-assisted indoor wireless sensor node self-positioning device according to the implementation mode of the present application may include: a data acquisition module 100, a preliminary positioning module 200, a first construction module 300, a local map selection module 400, a second construction module 500, a third construction module 600 and a precise positioning module 700.
[0196] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the data acquisition module 100 can be configured to obtain a distance measurement value set by measuring the distance between multiple wireless sensor nodes, and obtain an indoor point cloud map through the laser radar; the preliminary positioning module 200 can be configured to obtain preliminary self-positioning results of multiple wireless sensor nodes in the point cloud map based on the distance measurement value set and the point cloud map; the first construction module 300 can be configured to construct a distance constraint cost function of the corresponding wireless sensor node based on the point cloud map and the distance measurement value set; the local map selection module 400 can be configured to In the point cloud map, the wireless sensor node is taken as the sphere center and a sphere range is selected with a specific radius as the local map of the corresponding wireless sensor node; the second construction module 500 can be configured to construct the space constraint cost function of the corresponding wireless sensor node in the local map by plane fitting and finite circular area approximation; the third construction module 600 is configured to construct a complete cost function based on the space constraint cost function and the distance constraint cost function; the precise positioning module 700 can be configured to obtain the value of the function variable with the preliminary self-positioning result as the initial value so that the complete cost function obtains the minimum value, as the precise self-positioning result of the wireless sensor node.
[0197] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the preliminary positioning module 200 can be configured to: obtain the node relative coordinates of multiple wireless sensor nodes according to the distance measurement value set; and perform preliminary coordinate system matching on the relative coordinates of multiple wireless sensor nodes and the point cloud map to obtain the self-positioning results of multiple wireless sensor nodes in the point cloud map. Among them, any technical solution in the prior art can be used as a method for preliminary coordinate system matching, for example, a method that can use sampling point pairs in the trajectory and calculate the conversion relationship between the two coordinate systems through the trajectory matching process.
[0198] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, specifically, in the first building module 300, the distance cost function can be constructed as: Among them, x n represents the nth wireless sensor node, x i represents the i-th wireless sensor node, Represents the distance between the nth wireless sensor node and the i-th wireless sensor node.
[0199] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, specifically, in the second building module 500, the space constraint cost function can be constructed as:
[0200]
[0201] Among them, the x of the wireless sensor node n is selected in the point cloud map n is the center of the sphere and the radius is R n A ball with a radius of M is used as the local map. n represents the local map, M represents the point cloud map, where
[0202] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, specifically, in the second construction module 500, the space constraint cost function can be further constructed as:
[0203]
[0204] Among them, h(X) represents the spatial constraint cost function,
[0205] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0206] n represents a wireless sensor node, n∈N,
[0207] x n represents the coordinates of wireless sensor node n,
[0208] represents the homogeneous coordinates of the wireless sensor node n,
[0209] P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map.
[0210] represents the plane representation parameters after normalizing multiple planes.
[0211]
[0212] represents the center of the fitted circular area,
[0213] r n,p represents the center of the fitted circular plane,
[0214] Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located,
[0215] Represents wireless sensor node x n The projection and center of the circle on the plane where the fitted circular area is located The distance
[0216] A represents a large numerical coefficient,
[0217] L is an operator, which means L(x)=max(0,x).
[0218] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, specifically, in the third construction module 600, the space constraint cost function and the distance constraint cost function can be added to obtain a complete cost function, that is, f(X)=h(X)+g(X). In other words, the complete cost function can be constructed as:
[0219]
[0220] Among them, f(X) represents the complete cost function,
[0221] N represents the set of wireless sensor nodes, N = {1, ..., N},
[0222] n represents a wireless sensor node, n∈N,
[0223] x n represents the coordinates of wireless sensor node n,
[0224] represents the homogeneous coordinates of the wireless sensor node n,
[0225] P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map.
[0226] represents the plane representation parameters after normalizing multiple planes.
[0227]
[0228] x nrepresents the wireless sensor node whose belonging plane is to be determined,
[0229] represents the center of the fitted circular area,
[0230] r n,p represents the center of the fitted circular plane,
[0231] Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located,
[0232] Represents point x n The projection and center of the circle on the plane where the fitted circular area is located The distance
[0233] A represents a large numerical coefficient,
[0234] L means L(x)=max(0,x),
[0235] x i represents the i-th wireless sensor node,
[0236] represents the distance between the nth wireless sensor node and the ith wireless sensor node,
[0237] σ R Indicates the ranging accuracy,
[0238] σ M Indicates the point cloud map accuracy.
[0239] In the laser radar-assisted indoor wireless sensor node self-positioning device according to the embodiment of the present application, the precise positioning module 700 can be configured as follows: for each node n, obtain the local cost function f n (X) Get the local minimum x n , loop to obtain the steps until the coordinates of each node x n Converge so that the complete cost function f(X) reaches the minimum value, and the node coordinates x of each node that makes the global cost function f(X) reach the minimum value n The value of is the precise self-positioning result of the wireless sensor node, where:
[0240]
[0241] According to the laser radar-assisted indoor wireless sensor node self-positioning method and device according to the implementation mode of the present application, specifically by modeling the point cloud map obtained by the laser radar, combined with the fact that the nodes are usually arranged on the surface of the object, the local point cloud map is additionally introduced to provide spatial constraints for each wireless sensor node. By plane fitting of the local point cloud map and approximation of the limited area of the object surface, the spatial constraints are combined with the distance constraints provided by the radio ranging results between the nodes, and the self-positioning results of each node under the joint constraints are solved. Simulation and actual measurement experiments have proved that by effectively combining the spatial constraints provided by the laser radar point cloud map according to the technical solution of the present application, the self-positioning accuracy of the sensor nodes can be further improved, and the relative position relationship between each node and the map environment can be further optimized, thereby achieving better matching and fusion of the sensor network and the real-time laser radar point cloud map.
[0242] In order to verify the beneficial effects of the laser radar-assisted indoor wireless sensor node self-positioning method, the inventor conducted a test experiment and confirmed that the introduction of spatial constraints can make each node converge to a result closer to the actual deployment position, thereby achieving a more significant improvement in absolute positioning accuracy. The inventor also conducted a field measurement comparison test between the traditional self-positioning method and the self-positioning method of the embodiment of the present invention. In the self-positioning results using traditional technology, although each node is matched to the vicinity of the actual deployment position, due to the complex indoor signal environment and limited node layout conditions, the positioning results of each node are still significantly different from the actual deployment results, but it is difficult to meet the needs of high-precision indoor positioning; the self-positioning results using the method of the present invention can significantly improve the positioning accuracy (relative positioning root mean square error is better than 20cm, and the relative positioning error refers to the error between the node spacing of the positioning result and the actual node distance), and can place the node very close to the actual deployment position (absolute positioning root mean square error is better than 40cm, and the absolute positioning error refers to the error between the node position of the positioning result and the actual node position), so as to achieve a good integration of the wireless sensor network and the real-time map scene, so that users can more accurately understand the distribution of each node in the map and support subsequent benchmark provision or error correction work on this basis.
[0243] The above specific implementations do not constitute a limitation on the protection scope of the present invention. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions may occur depending on design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. A laser radar-assisted indoor wireless sensor node self-positioning method, which is used to assist multiple wireless sensor nodes to self-position indoors by using a laser radar, wherein the indoors contains at least one object, and the multiple wireless sensor nodes are arranged on the surface of the at least one object. The laser radar-assisted indoor wireless sensor node self-positioning method include: A distance measurement value set is obtained by measuring the distance between the multiple wireless sensor nodes, and a point cloud map of the room is obtained by using the laser radar; Obtaining preliminary self-positioning results of the plurality of wireless sensor nodes in the point cloud map according to the distance measurement value set and the point cloud map; Constructing a distance constraint cost function of a corresponding wireless sensor node based on the point cloud map and the distance measurement value set; In the point cloud map, the wireless sensor node is taken as the sphere center and a sphere range is selected with a specific radius as a local map of the corresponding wireless sensor node; Constructing a spatial constraint cost function of a corresponding wireless sensor node in the local map by plane fitting and finite circular area approximation; Constructing a complete cost function based on the space constraint cost function and the distance constraint cost function; and The preliminary self-positioning result is used as an initial value to obtain the value of the function variable that makes the complete cost function obtain the minimum value, and the value is used as the accurate self-positioning result of the wireless sensor node.
2. The laser radar-assisted indoor wireless sensor node self-positioning method according to claim 1, in, Obtaining node relative coordinates of the plurality of wireless sensor nodes according to the distance measurement value set; as well as The relative coordinates of the multiple wireless sensor nodes are preliminarily matched with the point cloud map to obtain the self-positioning results of the multiple wireless sensor nodes in the point cloud map.
3. The laser radar-assisted indoor wireless sensor node self-positioning method according to claim 1 or 2, in, The spatial constraint cost function is constructed as: Among them, h′(X) represents the spatial constraint cost function, N represents the set of wireless sensor nodes, N = {1, ..., N}, n represents the wireless sensor node, n∈N, x n represents the coordinates of wireless sensor node n, represents the homogeneous coordinates of the wireless sensor node n, P n represents a set of multiple planes to which any wireless sensor node n is attached obtained by plane fitting in the local map, represents the plane representation parameter after normalizing the multiple planes, Wherein, in the point cloud map, the x of the wireless sensor node n is selected n is the center of the sphere and the radius is R n is a sphere with radius M as the local map, n represents the local map, M represents the point cloud map, where 4. The laser radar-assisted indoor wireless sensor node self-positioning method according to claim 3, in, The spatial constraint cost function is further constructed as: Among them, h(X) represents the spatial constraint cost function, N represents the set of wireless sensor nodes, N = {1, ..., N}, n represents the wireless sensor node, n∈N, x n represents the coordinates of wireless sensor node n, represents the homogeneous coordinates of the wireless sensor node n, P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map. represents the plane representation parameter after normalizing the multiple planes, represents the center of the fitted circular area, r n,p represents the center of the fitted circular plane, Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located, Represents wireless sensor node x n The projection and center of the circle on the plane where the fitted circular area is located The distance A represents a large numerical coefficient, L is an operator, which means L(x)=max(0,x).
5. The laser radar-assisted indoor wireless sensor node self-positioning method according to any one of claims 1 to 4, in, The distance cost function is constructed as: Among them, x n represents the nth wireless sensor node, x i represents the i-th wireless sensor node, Represents the distance between the nth wireless sensor node and the i-th wireless sensor node.
6. The laser radar-assisted indoor wireless sensor node self-positioning method according to any one of claims 1 to 5, in, The complete cost function is constructed as: Among them, f(X) represents the complete cost function, N represents the set of wireless sensor nodes, N = {1, ..., N}, n represents the wireless sensor node, n∈N, x n represents the coordinates of wireless sensor node n, represents the homogeneous coordinates of the wireless sensor node n, P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map. represents the plane representation parameter after normalizing the multiple planes, x n represents the wireless sensor node whose belonging plane is to be determined, represents the center of the fitted circular area, r n,p represents the center of the fitted circular plane, Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located, Represents point x n The projection and center of the circle on the plane where the fitted circular area is located The distance A represents a large numerical coefficient, L means L(x)=max(0,x), x i represents the i-th wireless sensor node, represents the distance between the nth wireless sensor node and the ith wireless sensor node, σ R Indicates the ranging accuracy, σ M Indicates the point cloud map accuracy.
7. The laser radar-assisted indoor wireless sensor node self-positioning method according to claim 6, in, The step of obtaining the value of the variable that makes the complete cost function obtain the minimum value includes: For each node n, obtain the local cost function f n (X) x that achieves a local minimum n , repeat the above steps until the coordinates x of each node are n Converge so that the complete cost function f(X) reaches the minimum value, where:
8. A laser radar-assisted indoor wireless sensor node self-positioning device, used for assisting multiple wireless sensor nodes to perform self-positioning indoors by laser radar, wherein the indoors contain at least one object, and the multiple wireless sensor nodes are arranged on the surface of the at least one object. The laser radar-assisted indoor wireless sensor node self-positioning device include: A data acquisition module is configured to obtain a distance measurement value set by measuring the distance between the multiple wireless sensor nodes, and obtain a point cloud map of the room by using the laser radar; A preliminary positioning module, configured to obtain preliminary self-positioning results of the plurality of wireless sensor nodes in the point cloud map according to the distance measurement value set and the point cloud map; A first construction module is configured to construct a distance constraint cost function of a corresponding wireless sensor node based on the point cloud map and the distance measurement value set; A local map selection module is configured to select a sphere range with a specific radius from the point cloud map with the wireless sensor node as the sphere center as the local map of the corresponding wireless sensor node; A second construction module is configured to construct a space constraint cost function of a corresponding wireless sensor node in the local map by plane fitting and finite circular area approximation; A third construction module is configured to construct a complete cost function based on the space constraint cost function and the distance constraint cost function; as well as The precise positioning module is configured to use the preliminary self-positioning result as an initial value, obtain the value of the function variable that makes the complete cost function obtain the minimum value, and use the value as the precise self-positioning result of the wireless sensor node.
9. According to the laser radar-assisted indoor wireless sensor node self-positioning device of claim 8, the preliminary positioning module is configured as follows: Obtaining node relative coordinates of the plurality of wireless sensor nodes according to the distance measurement value set; and The relative coordinates of the multiple wireless sensor nodes are preliminarily matched with the point cloud map to obtain the self-positioning results of the multiple wireless sensor nodes in the point cloud map.
10. The laser radar-assisted indoor wireless sensor node self-positioning device according to claim 8 or 9, in, The spatial constraint cost function is constructed as: Among them, h′(X) represents the spatial constraint cost function, N represents the set of wireless sensor nodes, N = {1, ..., N}, n represents the wireless sensor node, n∈N, x n represents the coordinates of wireless sensor node n, represents the homogeneous coordinates of the wireless sensor node n, P n represents a set of multiple planes to which any wireless sensor node n is attached obtained by plane fitting in the local map, represents the plane representation parameter after normalizing the multiple planes, Wherein, in the point cloud map, the x of the wireless sensor node n is selected n is the center of the sphere and the radius is R n is a sphere with radius M as the local map, n represents the local map, M represents the point cloud map, where 11. The laser radar-assisted indoor wireless sensor node self-positioning device according to claim 10, in, The spatial constraint cost function is further constructed as: Among them, h(X) represents the spatial constraint cost function, N represents the set of wireless sensor nodes, N = {1, ..., N}, n represents a wireless sensor node, n∈N, x n represents the coordinates of wireless sensor node n, represents the homogeneous coordinates of the wireless sensor node n, P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map. represents the plane representation parameter after normalizing the multiple planes, represents the center of the fitted circular area, r n,p represents the center of the fitted circular plane, Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located, Represents wireless sensor node x n The projection and center of the circle on the plane where the fitted circular area is located The distance A represents a large numerical coefficient, L is an operator, which means L(x)=max(0,x).
12. The laser radar-assisted indoor wireless sensor node self-positioning device according to any one of claims 8 to 11, in, The distance cost function is constructed as: Among them, x n represents the nth wireless sensor node, x i represents the i-th wireless sensor node, Represents the distance between the nth wireless sensor node and the i-th wireless sensor node.
13. The laser radar-assisted indoor wireless sensor node self-positioning device according to any one of claims 8 to 12, in, The complete cost function is constructed as: Among them, f(X) represents the complete cost function, N represents the set of wireless sensor nodes, N = {1, ..., N}, n represents a wireless sensor node, n∈N, x n represents the coordinates of wireless sensor node n, represents the homogeneous coordinates of the wireless sensor node n, P n represents the set of multiple planes to which any wireless sensor node n is attached, obtained by plane fitting in the local map. represents the plane representation parameter after normalizing the multiple planes, x n represents the wireless sensor node whose belonging plane is to be determined, represents the center of the fitted circular area, r n,p represents the center of the fitted circular plane, Represents wireless sensor node x n The distance between the point and the plane where the fitted circular area is located, Represents point x n The projection and center of the circle on the plane where the fitted circular area is located The distance A represents a large numerical coefficient, L means L(x)=max(0,x), x i represents the i-th wireless sensor node, represents the distance between the nth wireless sensor node and the ith wireless sensor node, σ R Indicates the ranging accuracy, σ M Indicates the point cloud map accuracy.
14. The laser radar-assisted indoor wireless sensor node self-positioning device according to any one of claims 8 to 13, in, The precise positioning module is configured as follows: For each node n, obtain the local cost function f n (X) x that achieves a local minimum n , repeat the above steps until the coordinates x of each node are n Converge so that the complete cost function f(X) reaches the minimum value, where:
Citation Information
Patent Citations
Node positioning method and system in wireless communication network based on target tracking
CN112996108A
Rapid relocation method for large-scale mapping scene based on point cloud
CN113763551A
Underground dark environment three-dimensional positioning mapping method based on three-dimensional laser radar
CN118534481A
Method for evaluating quality of point cloud map based on matching
US20240185595A1
