Node positioning method, apparatus and electronic device

Abstract

The present application relates to the technical field of unmanned cluster cooperative positioning, and discloses a node positioning method, device and electronic equipment, wherein in the case that a to-be-positioned node is located in a denial environment, inertia measurement data of the to-be-positioned node is determined, a reference position of an anchor node is determined, a node distance between the anchor node and the to-be-positioned node is determined, a relative displacement parameter of the to-be-positioned node is determined, and under the constraint of the relative displacement parameter, the node position of the to-be-positioned node is determined according to the reference position and the node distance. The present application can accurately position the to-be-positioned node in a satellite navigation denial environment. In addition, the present application does not need to rely on pre-deployed fixed anchor nodes, and enhances the environmental adaptability of the positioning of the to-be-positioned node. Meanwhile, cooperative positioning is performed through the node distance and inertia measurement data, and the problem that the positioning cumulative error cannot be eliminated in the related art is solved.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) positioning technology, specifically to node positioning methods, devices, and electronic equipment. Background Technology

[0002] Currently, unmanned swarms typically employ satellite navigation-based methods for cooperative positioning and heading control. However, in typical scenarios such as urban canyons and underground environments, this method cannot provide stable and reliable position information for unmanned equipment due to GNSS (Global Navigation Satellite System) obstruction or interference from adverse environments. For cooperative positioning of unmanned swarms in satellite navigation denied environments, positioning technologies based on communication equipment can achieve high-precision positioning with multiple anchor nodes. However, these technologies heavily rely on pre-deployed fixed anchor nodes and are susceptible to signal obstruction and multipath effects, making rapid deployment in unknown dynamic environments difficult and unable to meet the high-precision positioning requirements of unmanned swarms. Inertial Measurement Unit (IMU)-based positioning technologies offer high positioning accuracy in short periods, but they cannot eliminate accumulated positioning errors. As flight time increases, the accumulation of positioning errors becomes more pronounced, severely impacting mission performance. Existing single-sensor-based positioning technologies have limitations and are ill-suited to the complex dynamic scenarios of large-scale, highly maneuverable unmanned swarms. Summary of the Invention

[0003] This invention provides a node positioning method, apparatus, and electronic device to solve the problem of being unable to accurately locate a node in a satellite navigation denied environment.

[0004] In a first aspect, the present invention provides a node localization method, the method comprising:

[0005] When the node to be located is in a denied environment, the inertial measurement data of the node to be located is determined, and the reference position of the anchor node located outside the denied environment is determined; the anchor node is an unmanned device.

[0006] The original distance between the anchor node and the node to be located is measured using the time-of-flight positioning method;

[0007] The original distance is corrected using a first-order Markov model to determine the node distance between the anchor node and the node to be located;

[0008] The relative displacement parameters of the node to be located are determined based on the inertial measurement data;

[0009] Determine the position observation equation of the node to be located; the position observation equation is constructed under the constraint of the relative displacement parameters of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located;

[0010] The node position of the node to be located is obtained based on the reference position, the node distance, the relative displacement parameter, and the position observation equation.

[0011] The node positioning method provided in this embodiment obtains the node distance between the node to be positioned and the anchor node. This distance is measured using time-of-flight positioning and corrected using a first-order Markov model, making the distance measurement more accurate. Furthermore, based on inertial measurement data obtained through an inertial measurement unit, it can accurately locate the node in a satellite navigation denied environment. Simultaneously, by using a freely movable anchor node to measure the distance to the node, it eliminates the need for pre-deployed fixed anchor nodes, enhancing the environmental adaptability of the node's positioning. Moreover, by using node distance and inertial measurement data for collaborative positioning, it solves the problem of positioning errors accumulating and diverging over time in related technologies.

[0012] In some optional implementations, the position observation equation is:

[0013] ;

[0014] in, For the node to be located in the th k The observation position of the node at any given moment; For the node to be located in the th k The Jacobian matrix of the position observation at time _____. This is the coordinate system transformation matrix. For the node to be located in the th k The position error matrix at time t. For the distance measurement between nodes in the first... k The noise matrix at time step 1.

[0015] In some optional implementations, the position observation equation is:

[0016] ;

[0017] in, For the node to be located in the th k The observation position of the node at any given moment; For the node to be located in the th k The Jacobian matrix of the position observation at time _____. This is the coordinate system transformation matrix. For the node to be located in the th k The position error matrix corresponding to the given time.

[0018] In some optional implementations, the position observation equation includes the position error matrix corresponding to the node to be located; the node localization method further includes:

[0019] Based on the posterior estimated position error matrix of the previous time step, the prior estimated position error matrix of the current time step is determined; the prior estimated position error matrix of the current time step is the position error matrix of the node to be located at the current time step.

[0020] Determine the gain coefficient at the current moment;

[0021] The prior estimated position error matrix at the current time is updated based on the gain coefficient at the current time to obtain the posterior estimated position error matrix at the current time.

[0022] In some optional implementations, obtaining the node position of the node to be located based on the reference position, the node distance, the relative displacement parameter, and the position observation equation includes:

[0023] Substituting the reference position, the node distance, and the relative displacement parameters into the position observation equation, the three-dimensional position closed-form solution of the node to be located is obtained;

[0024] Based on the posterior estimated position error matrix at the current moment, the three-dimensional position closed solution is corrected to obtain the node position of the node to be located.

[0025] In some optional implementations, updating the prior estimated position error matrix at the current time based on the gain coefficient at the current time to obtain the posterior estimated position error matrix at the current time includes:

[0026] After determining the closed-form solution of the three-dimensional position of the node to be located at the current time, the position measurement residual between the closed-form solution of the three-dimensional position at the current time and the theoretical observation position at the current time is determined.

[0027] Based on the gain coefficient and position measurement residual at the current moment, the prior estimated position error matrix at the current moment is updated to obtain the posterior estimated position error matrix at the current moment.

[0028] In some optional implementations, determining the gain coefficient at the current moment includes:

[0029] Based on the posterior estimated covariance of the previous time step, determine the prior estimated covariance of the current time step;

[0030] Based on the prior estimated covariance at the current moment, determine the gain coefficient at the current moment;

[0031] The method further includes:

[0032] The prior estimated covariance at the current time is updated based on the gain coefficient at the current time to obtain the posterior estimated covariance at the current time.

[0033] The node positioning method provided in this embodiment obtains the node distance between the node to be positioned and the anchor node through time-of-flight ranging, and calculates the position of the node to be positioned based on the inertial measurement data obtained by the inertial measurement unit. In a satellite navigation denied environment, it can calculate the three-dimensional closed-form solution of the node to be positioned in real time using the principle of spherical intersection. After calculating the three-dimensional closed-form solution, it completes the state estimation of the node to be positioned using the system position observation model through an error state filter, and corrects the three-dimensional closed-form solution of the node to be positioned based on the state estimation, so as to obtain the accurate positioning of the node to be positioned. This method suppresses the cumulative divergence rate of the position error of the node to be positioned and solves the problem of the positioning error accumulating and diverging over time in related technologies.

[0034] In a second aspect, the present invention provides a node positioning device, the device comprising:

[0035] The reference position module is used to determine the inertial measurement data of the node to be located when it is in a denied environment, and to determine the reference position of the anchor node located outside the denied environment; the anchor node is an unmanned device.

[0036] The node distance module is used to measure the original distance between the anchor node and the node to be located using the time-of-flight positioning method; the original distance is corrected by a first-order Markov model to determine the node distance between the anchor node and the node to be located.

[0037] The displacement parameter module is used to determine the relative displacement parameters of the node to be located based on inertial measurement data.

[0038] The position determination module is used to determine the node to be located; the position observation equation is constructed under the constraint of the relative displacement parameters of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located; the node position of the node to be located is obtained based on the reference position, node distance, relative displacement parameters and position observation equation.

[0039] Thirdly, the present invention provides an electronic device, comprising: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the computer instructions to perform the node positioning method of the first aspect or any corresponding embodiment described above.

[0040] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions for causing a computer to perform the node positioning method of the first aspect or any corresponding embodiment thereof.

[0041] Fifthly, the present invention provides a computer program product, including computer instructions for causing a computer to execute the node positioning method of the first aspect or any corresponding embodiment thereof. Attached Figure Description

[0042] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0043] Figure 1 This is a schematic diagram of an application scenario according to an embodiment of the present invention;

[0044] Figure 2 This is a schematic diagram of the first type of node positioning method according to an embodiment of the present invention;

[0045] Figure 3 This is a schematic diagram of the second process of the node positioning method according to an embodiment of the present invention;

[0046] Figure 4 This is a schematic diagram of a time-of-flight positioning method according to an embodiment of the present invention;

[0047] Figure 5 This is a schematic diagram of spatial positional relationships at different times according to an embodiment of the present invention;

[0048] Figure 6 This is a structural block diagram of a node positioning device according to an embodiment of the present invention;

[0049] Figure 7 This is a schematic diagram of the hardware structure of an electronic device according to an embodiment of the present invention. Detailed Implementation

[0050] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0051] It is understood that before using the technical solutions disclosed in the various embodiments of the present invention, users should be informed of the types, scope of use, and usage scenarios of the personal information involved in the present invention and their authorization should be obtained in accordance with relevant laws and regulations through appropriate means.

[0052] As an optional application scenario of this invention, such as Figure 1 As shown, the node positioning system may include at least one node to be located, at least one anchor node, and at least one server. Figure 1 The system is illustrated in the example, including a node to be located 101, an anchor node 102, and a server 103.

[0053] Specifically, the node to be located 101 can be an unmanned device that needs to be located, such as a drone, and the anchor node 102 can be another unmanned device. The server 103 can be an independent physical server or terminal device, or a server cluster or distributed system, etc.

[0054] This invention provides a node positioning method that, by obtaining the node distance between the node to be positioned and the anchor node, and based on the inertial measurement data obtained through the inertial measurement unit, enables precise positioning of the node to be positioned in a satellite navigation denied environment.

[0055] According to an embodiment of the present invention, a node localization method embodiment is provided. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0056] This embodiment provides a node localization method, which can be used for the aforementioned unmanned devices, such as drones and unmanned vehicles, or for the remote control system of drones, such as servers or terminal devices. Figure 2 This is a flowchart of a node localization method according to an embodiment of the present invention, such as... Figure 2 As shown, the process includes the following steps:

[0057] Step S201: When the node to be located is in a denied environment, determine the inertial measurement data of the node to be located and determine the reference position of the anchor node located outside the denied environment.

[0058] The node to be located is the node that needs to be located. This node can be an unmanned device, such as a drone. The denied environment is an environment where the Global Navigation Satellite System (GNSS) signal cannot be reliably received by the terminal device (e.g., the node to be located in this embodiment), or even if received, it cannot be used for effective positioning. Examples include urban areas, canyons, indoor environments, or underground locations. Because GNSS signals are blocked or interfered with by adverse environmental factors, stable and reliable position information cannot be provided to the unmanned device, i.e., precise positioning is impossible. Inertial measurement data refers to sensor readings output by the IMU (Inertial Measurement Unit). This inertial measurement data can include data from a three-axis gyroscope, a three-axis accelerometer, etc. During the positioning process, the reference position of another node, the anchor node, needs to be determined. The anchor node can also be a positioning-capable unmanned device, such as another drone. Since the anchor node needs to locate the node to be located, and its specific reference position needs to be precise, the anchor node needs to be located outside the denied environment to ensure it can correctly upload its reference position.

[0059] Step S202: Determine the node distance between the anchor node and the node to be located.

[0060] During the ranging process, data exchange can be established between the anchor node and the node to be located to determine the distance between them. Since the obstructed environment only affects the global navigation satellite system signal, the data exchange between the anchor node and the node to be located via electromagnetic signals is generally unaffected, thus allowing the determination of the distance between them. For example, the distance can be obtained by using Time of Flight (TOF) positioning based on communication equipment.

[0061] Step S203: Determine the relative displacement parameters of the node to be located based on the inertial measurement data.

[0062] As mentioned earlier, inertial measurement data (IMU) of the node to be located can be obtained. Based on this IMU data, the relative displacement parameters of the node at various time points can be determined. For example, using the three-axis gyroscope and three-axis accelerometer data of the node, the velocity and position differential equations of the node obtained from the inertial measurement data over a continuous time period can be derived. Substituting the multiple time points to be located into the velocity and position differential equations, the relative displacement parameters between each time point can be obtained. Time point Time point Relative displacement parameters between , ; among the time points With time point Adjacent, time point With time point Adjacent, That is, the point in time. With time point The vector of positional changes between them can be used as a relative displacement parameter. That is, the point in time. With time point The relative displacement parameters between them.

[0063] Step S204: Under the constraint of relative displacement parameters, determine the node position of the node to be located based on the reference position and node distance.

[0064] Based on the reference position of the anchor node at the corresponding time point and the node distance between the anchor node and the node to be located at that time point, a sphere can be drawn with the anchor node as the center and the node distance as the radius. The positions on the sphere represent the possible node positions of the node to be located at the corresponding time point. Then, by using relative displacement parameters as constraints, the node position of the node to be located is determined on the sphere based on the relative displacement parameters. This node position may have errors, which can be corrected to obtain a more accurate node position.

[0065] The node positioning method provided in this embodiment can accurately locate the node to be positioned in a satellite navigation denied environment by obtaining the node distance between the node to be positioned and the anchor node and using the inertial measurement data obtained by the inertial measurement unit. At the same time, the distance to the node to be positioned is measured by a freely moving anchor node, which does not rely on pre-deployed fixed anchor nodes, thus enhancing the environmental adaptability of the positioning of the node to be positioned. In addition, the collaborative positioning using node distance and inertial measurement data solves the problem of positioning error accumulating and diverging over time in related technologies.

[0066] This embodiment provides a node localization method, which can be used for the aforementioned unmanned devices, such as drones and unmanned vehicles, or for the remote control system of drones, such as servers or terminal devices. Figure 3 This is a flowchart of a node localization method according to an embodiment of the present invention, such as... Figure 3 As shown, the process includes the following steps:

[0067] Step S301: When the node to be located is in a denied environment, determine the inertial measurement data of the node to be located and determine the reference position of the anchor node located outside the denied environment.

[0068] Please see details Figure 2 Step S201 of the illustrated embodiment will not be described again here.

[0069] Step S302: Determine the node distance between the anchor node and the node to be located.

[0070] Please see details Figure 2 Step S202 of the illustrated embodiment will not be described again here.

[0071] In some alternative implementations, step S202, “determining the node distance between the anchor node and the node to be located,” includes steps a1 and a2.

[0072] Step a1: Measure the original distance between the anchor node and the node to be located using the time-of-flight positioning method.

[0073] Step a2: Correct the original distance to determine the node distance between the anchor node and the node to be located.

[0074] In determining the distance between the anchor node and the node to be located, the time-of-flight positioning method can be used to measure the initial distance between the anchor node and the node to be located. The node to be located sends a ranging request to the anchor node, and after the signal's time of flight... Then, the anchor node receives this ranging request. The anchor node experiences a reception delay. Then, a reply message is sent to the node to be located. The sending and receiving times of the node to be located are recorded as follows: and The sending and receiving times of the anchor node are respectively and The time it takes for the signal to travel between nodes during the entire ranging process It can be represented as:

[0075] ;

[0076] in, It can be represented as , It can be represented as Then the formula can also be expressed as:

[0077] ;

[0078] At this point, the original distance between the anchor node and the node to be located... d It can be represented as ,in c It is the speed of light.

[0079] Figure 4 This is a schematic diagram of the time-of-flight positioning method, such as... Figure 4As shown, the node to be located sends a ranging request to the anchor node, and the sending time is... The anchor node receives the ranging request at the time specified in the original text. The anchor node returns the ranging request in time. The time it takes for the positioning node to receive this return request is ,in, That is , That is The picture This refers to the signal flight time.

[0080] After completing the original distance d After the calculation, the clock skew of the nodes can be considered in relation to the original distance. d The impact of this can be assessed, and further corrections can be made. For example, a first-order Markov model can be used to iterate frame by frame to correct the time-of-flight measurement for each frame. This compensates for ranging errors caused by clock skew.

[0081] Step S303: Determine the relative displacement parameters of the node to be located based on the inertial measurement data.

[0082] Please see details Figure 2 Step S203 of the illustrated embodiment will not be described again here.

[0083] Step S304: Under the constraint of relative displacement parameters, determine the node position of the node to be located based on the reference position and node distance.

[0084] Specifically, step S304, "Under the constraint of relative displacement parameters, determine the node position of the node to be located based on the reference position and node distance," includes steps S3041 and S3042.

[0085] Step S3041: Determine the position observation equation of the node to be located. The position observation equation is constructed under the constraint of the relative displacement of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located.

[0086] Step S3042: Based on the reference position, node distance, relative displacement parameters, and position observation equation, obtain the node position of the node to be located.

[0087] In determining the node positions, a position observation equation can be first determined based on the reference position, node distance, and relative displacement parameters. For ease of description, this equation is referred to as the first position observation equation. Specifically, for any given current time point where the position needs to be determined... It can be done through the current time point Corresponding anchor node position and the current time point Corresponding node distance and at the current point in time The last two time points , Corresponding anchor node position , Distance to nodes determined , The equation for the first position observation is:

[0088] ;

[0089] ;

[0090] .

[0091] Among them, the latter two time points , The corresponding node position to be located , It can be represented as:

[0092] ;

[0093] .

[0094] in , The current time point The last two time points , The corresponding relative displacement parameters can be used to obtain the time point. up to the time point and time points up to the time point The relative displacement.

[0095] Then place the anchor node at , and The corresponding reference position , and The corresponding node distance, and and Corresponding relative displacement parameters , Substituting these values ​​into the first position observation equation yields the corresponding solution. This solution can then be used as the location of the node to be located at the current time point. Corresponding node observation position The node observation position at this time It is a value with error, which can be represented as:

[0096] .

[0097] Figure 5 This is a schematic diagram of spatial relationships at different times, at three points in time. , and Time point Corresponding anchor node position Location of the node to be located and the distance between nodes Time point and The corresponding anchor node position, the position of the node to be located, and the distance and time point between the nodes. Similarly, I will not go into details here.

[0098] In some alternative implementations, the second position observation equation can be obtained by linearizing the above formula (i.e., the first position observation equation) into a Taylor expansion, resulting in the linearized Taylor expansion second position observation equation:

[0099] .

[0100] in, For the node to be located at time point The node observation location. For the node to be located at time point The corresponding position observation Jacobian matrix is ​​the position observation Jacobian matrix obtained by linearizing the above formula using Taylor expansion.

[0101] That is, step S3041 above specifically involves: determining the first position observation equation of the node to be located, and performing a linearized Taylor expansion on the first position observation equation to determine the second position observation equation; the first position observation equation is constructed under the constraint of the relative displacement parameters of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located.

[0102] The coordinate transformation matrix, used for transforming from the Earth coordinate system to the navigation coordinate system, can be expressed as follows:

[0103]

[0104] in f For Earth's ellipsoid, h Altitude Let be the radius of curvature of the Earth's orbit. L Latitude Longitude.

[0105] For the node to be located at time point The corresponding position error matrix is ​​the prior estimate error matrix of the position error, which is used to characterize the prediction error state. The distance between nodes (i.e., between the node to be located and the anchor node) is measured at a specific time point. The noise matrix can be ignored during calculation when the distance noise matrix between nodes is very small; that is, the second position observation equation can be simplified to:

[0106] .

[0107] In some optional implementations, the second position observation equation includes the position error matrix corresponding to the node to be located, and the node localization method further includes steps b1 to b3.

[0108] Step b1: Based on the posterior estimated position error matrix from the previous time step, determine the prior estimated position error matrix for the current time step. The prior estimated position error matrix for the current time step is the position error matrix corresponding to the node to be located at the current time step.

[0109] Step b2: Determine the gain coefficient at the current moment.

[0110] Step b3: Update the prior estimated position error matrix at the current time based on the gain coefficient at the current time to obtain the posterior estimated position error matrix at the current time.

[0111] Using the current moment as the time point For example, the previous moment is the time point. After determining the time point of the node to be located. Corresponding position error matrix In this case, we can first determine the previous moment (i.e., the time point). The posterior estimated location error matrix Among them, the posterior estimated location error matrix This is a quantitative description of the position error state after the filter completes the measurement update. It is based on the posterior estimated position error matrix. This allows us to determine the current time (i.e., the point in time). The prior estimated position error matrix The prior estimated position error matrix That is, the node to be located in the above formula at time point. Corresponding position error matrix .

[0112] Among them, based on the previous moment (i.e., the time point) The posterior estimated location error matrix Determine the current time (i.e., the point in time) The prior estimated position error matrix The process can be obtained using the following formula:

[0113] .

[0114] in, The system's state transition matrix is ​​in discrete form, i.e., from time point... up to the time point The state transition matrix is ​​generally a system in discrete form. This is the input matrix for system noise control. For time points The corresponding system noise matrix.

[0115] Generally speaking, due to the system noise matrix The corresponding noise is generally small and can be ignored. The above formula can also be simplified to:

[0116] .

[0117] To facilitate at the next moment (i.e., time point) It can use the current time (i.e., the point in time) normally. The posterior estimated location error matrix (i.e., for a point in time) In other words, (Given the posterior estimated position error matrix from the previous time step), we can first determine the current time step (i.e., time point). Gain coefficient The gain coefficient is a parameter used in the filtering process to determine how much new observation data should be trusted to correct previous predictions when updating the state.

[0118] After obtaining the gain coefficient Then, based on the current time (i.e., the point in time) Gain coefficient Update the prior estimated position error matrix at the current time step. This yields the posterior estimated position error matrix at the current time. Used to determine the next moment (i.e., time point). The corresponding position error matrix .

[0119] In some optional implementations, step S3042, "obtaining the node position of the node to be located based on the reference position, node distance, relative displacement parameters, and the second position observation equation," includes steps c1 and c2.

[0120] Step c1: Substitute the reference position, node distance, and relative displacement parameters into the first position observation equation to obtain the three-dimensional closed-form solution of the node to be located.

[0121] Step c2: Based on the posterior estimated position error matrix calculated from the second position observation equation, the three-dimensional position closed solution is corrected to obtain the node position of the node to be located.

[0122] As mentioned earlier, the anchor node is located at , and Corresponding reference position , and The corresponding node distance, and Corresponding relative displacement parameters , Substituting these values ​​into the first position observation equation yields the solution result that can be used as the node observation position of the node to be located at the current time point. However, this solution result is a node observation position with errors; therefore, this solution result with errors can be referred to as... ,in This refers to the closed-form solution of the three-dimensional position of the node to be located, obtained through the first position observation equation. Then, the posterior estimated position error matrix for the current time is calculated based on the second position observation equation. The closed-form solution of the three-dimensional position is obtained through a filter. By performing correction, the node position of the node to be located can be obtained. In this embodiment, the above process can calculate the node position of the node to be located using the following formula. :

[0123] , where C( k () is a preset coefficient filtering matrix used to filter out the position error matrix. The parameters related to position. In this embodiment, C( k ) can be expressed as Where I is the identity matrix, It is a zero matrix with 3 rows and 12 columns.

[0124] Among them, node position It can be represented as:

[0125] .

[0126] In some alternative implementations, step b2, “determining the gain coefficient at the current moment,” includes steps b21 and b22.

[0127] Step b21: Determine the prior estimated covariance at the current time based on the posterior estimated covariance of the previous time step.

[0128] Step b22: Determine the gain coefficient for the current time based on the prior estimated covariance at the current time.

[0129] Calculate the current moment (i.e., the point in time) The corresponding gain coefficient At that time, it can be based on the previous moment (i.e., the point in time). The corresponding posterior estimated covariance Calculate the prior estimate covariance at the current time. Specifically, it can be calculated using the following formula:

[0130] .

[0131] in, The previous moment (i.e., a point in time) The noise covariance matrix is ​​a mathematical tool used in the filtering process to quantitatively describe the magnitude and correlation of system model errors.

[0132] Obtain the prior estimate of covariance at the current moment. The gain coefficient at the current moment can then be calculated using the following formula. :

[0133] .

[0134] in, That is, H( k ) represents the Jacobian matrix of the location observation. And coordinate system transformation matrix Matrix product; To measure the noise covariance; T represents the matrix transpose operation, and -1 represents the matrix inverse operation.

[0135] In some alternative implementations, the gain coefficient at the current moment is obtained. The node localization method then includes step b23.

[0136] Step b23: Update the prior estimated covariance at the current time based on the gain coefficient at the current time to obtain the posterior estimated covariance at the current time.

[0137] After obtaining the gain coefficient at the current moment, it can be used to determine the gain coefficient. Update the current time (i.e., the point in time) Prior estimate of covariance Used to calculate the next moment (i.e., time point). Gain coefficient ;

[0138] The calculation method can use the following formula:

[0139] , where I is the identity matrix.

[0140] In some optional implementations, step b3, "updating the prior estimated position error matrix at the current time based on the gain coefficient at the current time to obtain the posterior estimated position error matrix at the current time", includes steps b31, b32, and b33.

[0141] Step b31: Calculate the theoretical observation position of the node to be located at the current time based on the prior estimated position error matrix at the current time.

[0142] Step b32: After determining the closed-form solution of the three-dimensional position of the node to be located at the current time, determine the position measurement residual between the closed-form solution of the three-dimensional position at the current time and the theoretical observation position at the current time.

[0143] Step b33: Based on the gain coefficient and position measurement residual at the current time, update the prior estimated position error matrix at the current time to obtain the posterior estimated position error matrix at the current time.

[0144] Upon obtaining the current moment (i.e., the point in time) The prior estimated position error matrix The current time (i.e., time point) is calculated using the position observation equation. Theoretical observation location .

[0145] The calculation method uses the following formula:

[0146] .

[0147] Determine the node to be located at the current time (i.e., time point). Theoretical observation location of the nodes Then, the closed-form solution of the three-dimensional position at the current moment can be calculated. Compared with theoretical observation position Position measurement residuals between Among them, the three-dimensional position closed solution The closed-form solution of the three-dimensional position of the node to be located is obtained by solving the principle of spherical intersection. Theoretical observation location The theoretical observation position can be obtained through the position observation equation. However, due to the cumulative error in IMU position estimation, the theoretical observation position is... Compare The error is large and inaccurate, so it is necessary to calculate the position measurement residual. For example, position measurement residuals. It can be .

[0148] Obtaining location measurement residuals Then, the gain coefficient at the current moment can be combined. Prior estimate of position error matrix at the current time By updating, the posterior estimated position error matrix at the current time can be obtained. In this embodiment, the above process can be used to calculate the posterior estimated position error matrix using the following formula. :

[0149] .

[0150] The node positioning method provided in this embodiment obtains the node distance between the node to be positioned and the anchor node through time-of-flight ranging, and calculates the position of the node to be positioned based on the inertial measurement data obtained by the inertial measurement unit. In a satellite navigation denied environment, it can calculate the three-dimensional closed-form solution of the node to be positioned in real time using the principle of spherical intersection. After calculating the three-dimensional closed-form solution, it completes the state estimation of the node to be positioned using the system position observation model through an error state filter, and corrects the three-dimensional closed-form solution of the node to be positioned based on the state estimation, so as to obtain the accurate positioning of the node to be positioned. This method suppresses the cumulative divergence rate of the position error of the node to be positioned and solves the problem of the positioning error accumulating and diverging over time in related technologies.

[0151] This embodiment also provides a node positioning device for implementing the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that performs a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0152] This embodiment provides a node positioning device, such as Figure 6 As shown, it includes:

[0153] The reference position module 601 is used to determine the inertial measurement data of the node to be located when the node to be located is in a denied environment, and to determine the reference position of the anchor node located outside the denied environment.

[0154] The node distance module 602 is used to determine the node distance between the anchor node and the node to be located.

[0155] The displacement parameter module 603 is used to determine the relative displacement parameters of the node to be located based on inertial measurement data.

[0156] The position determination module 604 is used to determine the node position of the node to be located based on the reference position and the node distance, under the constraint of relative displacement parameters.

[0157] In some alternative implementations, the location determination module 604 includes:

[0158] The observation equation construction submodule is used to determine the position observation equations of the node to be located. The position observation equations are constructed under the constraint of the relative displacement of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located.

[0159] The position determination submodule is used to obtain the node position of the node to be located based on the reference position, node distance, relative displacement parameters, and position observation equation.

[0160] In some alternative implementations, the position observation equation is:

[0161] .

[0162] or, .

[0163] in, For the node to be located in the th k The node observation position at any given time. For the node to be located in the th k The positional observation Jacobian matrix at time [time] This is the coordinate system transformation matrix. For the node to be located in the th k The position error matrix at time t. For the distance measurement between nodes in the first... k The noise matrix at time step 1.

[0164] In some optional implementations, the position observation equation includes the position error matrix corresponding to the node to be located, and the node positioning device further includes:

[0165] The prior position error module is used to determine the prior estimated position error matrix for the current time step based on the posterior estimated position error matrix from the previous time step. The prior estimated position error matrix for the current time step is the position error matrix corresponding to the node to be located at the current time step.

[0166] The gain coefficient module is used to determine the gain coefficient at the current moment.

[0167] The posterior position error module is used to update the prior estimated position error matrix at the current time based on the gain coefficient at the current time, so as to obtain the posterior estimated position error matrix at the current time.

[0168] In some optional implementations, the location determination submodule includes:

[0169] The position calculation unit is used to substitute the reference position, node distance, and relative displacement parameters into the position observation equation to calculate the three-dimensional closed-form solution of the node to be located.

[0170] The position correction unit is used to estimate the position error matrix posteriorly at the current time, correct the three-dimensional position closed solution, and obtain the node position of the node to be located.

[0171] In some alternative implementations, the posterior position error module includes:

[0172] The residual submodule is used to determine the position measurement residual between the three-dimensional position closed solution at the current time and the theoretical observation position at the current time after determining the three-dimensional position closed solution of the node to be located at the current time.

[0173] The posterior position error submodule is used to update the prior estimated position error matrix at the current time based on the gain coefficient and position measurement residual at the current time, so as to obtain the posterior estimated position error matrix at the current time.

[0174] In some alternative implementations, the gain coefficient module includes:

[0175] The prior covariance submodule is used to determine the prior estimated covariance at the current time step based on the posterior estimated covariance at the previous time step.

[0176] The gain coefficient submodule is used to determine the gain coefficient at the current time based on the prior estimated covariance at the current time.

[0177] The gain coefficient module also includes:

[0178] The posterior covariance submodule is used to update the prior estimated covariance at the current time based on the gain coefficient at the current time, so as to obtain the posterior estimated covariance at the current time.

[0179] In some alternative implementations, the node distance module 602 includes:

[0180] The raw distance submodule is used to measure the raw distance between the anchor node and the node to be located using the time-of-flight positioning method.

[0181] The distance correction submodule is used to correct the original distance and determine the node distance between the anchor node and the node to be located.

[0182] The node positioning device provided in this embodiment of the invention can execute the node positioning method provided in any embodiment of the invention, and has the corresponding functional modules and beneficial effects for executing the method. Further functional descriptions of the various modules and units described above are the same as in the corresponding embodiments described above, and will not be repeated here.

[0183] Figure 7 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention.

[0184] The following is a detailed reference. Figure 7 This diagram illustrates a suitable structural schematic for implementing an electronic device according to embodiments of the present invention. The electronic device may include a processor (e.g., a central processing unit, graphics processor, etc.) 701, which can perform various appropriate actions and processes based on a program stored in read-only memory (ROM) 702 or a program loaded from memory 708 into random access memory (RAM) 703. The RAM 703 also stores various programs and data required for the operation of the electronic device. The processor 701, ROM 702, and RAM 703 are interconnected via a bus 704. An input / output (I / O) interface 705 is also connected to the bus 704.

[0185] Typically, the following devices can be connected to I / O interface 705: input devices 706 including, for example, touchscreens, touchpads, keyboards, mice, cameras, microphones, accelerometers, gyroscopes, etc.; output devices 707 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; memory devices 708 including, for example, magnetic tapes, hard disks, etc.; and communication devices 709. Communication device 709 allows electronic devices to exchange data via wireless or wired communication with other devices. Although Figure 7 Electronic devices with various devices are shown, but it should be understood that it is not required to implement or have all of the devices shown, and more or fewer devices may be implemented or have instead.

[0186] In particular, according to embodiments of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a non-transitory computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device 709, or installed from a memory 708, or installed from a ROM 702. When the computer program is executed by the processor 701, it performs the functions defined in the node localization method of the embodiments of the present invention.

[0187] Figure 7The electronic device shown is merely an example and should not be construed as limiting the functionality and scope of use of the embodiments of the present invention.

[0188] This invention also provides a computer-readable storage medium. The methods described above according to embodiments of the invention can be implemented in hardware or firmware, or implemented as computer code that can be recorded on a storage medium, or implemented as computer code downloaded via a network and originally stored on a remote storage medium or a non-transitory machine-readable storage medium and then stored on a local storage medium. Thus, the methods described herein can be processed by software stored on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, optical disk, read-only memory, random access memory, flash memory, hard disk, or solid-state drive, etc.; further, the storage medium can also include combinations of the above types of memory. It is understood that computers, processors, microprocessor controllers, or programmable hardware include storage components capable of storing or receiving software or computer code. When the software or computer code is accessed and executed by the computer, processor, or hardware, the node location method shown in the above embodiments is implemented.

[0189] A portion of this invention can be applied as a computer program product, such as computer program instructions, which, when executed by a computer, can invoke or provide the methods and / or technical solutions according to the invention through the operation of the computer. Those skilled in the art will understand that the forms in which computer program instructions exist in a computer-readable medium include, but are not limited to, source files, executable files, installation package files, etc. Correspondingly, the ways in which computer program instructions are executed by a computer include, but are not limited to: the computer directly executing the instructions, or the computer compiling the instructions and then executing the corresponding compiled program, or the computer reading and executing the instructions, or the computer reading and installing the instructions and then executing the corresponding installed program. Here, the computer-readable medium can be any available computer-readable storage medium or communication medium accessible to a computer.

[0190] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A node localization method, characterized in that, The method includes: When the node to be located is in a denied environment, the inertial measurement data of the node to be located is determined, and the reference position of the anchor node located outside the denied environment is determined; the anchor node is an unmanned device. The original distance between the anchor node and the node to be located is measured using the time-of-flight positioning method; The original distance is corrected using a first-order Markov model to determine the node distance between the anchor node and the node to be located; The relative displacement parameters of the node to be located are determined based on the inertial measurement data; A first position observation equation is determined for the node to be located, and a linearized Taylor expansion is performed on the first position observation equation to determine a second position observation equation; the first position observation equation is constructed under the constraint of the relative displacement parameters of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located; The node position of the node to be located is obtained based on the reference position, the node distance, the relative displacement parameter, and the second position observation equation.

2. The method according to claim 1, characterized in that, The second position observation equation is: ； in, For the node to be located in the th k The observation position of the node at any given moment; For the node to be located in the th k The Jacobian matrix of the position observation at time _____. This is the coordinate system transformation matrix. For the node to be located in the th k The position error matrix at time t. For the distance measurement between nodes in the first... k The noise matrix at time step 1.

3. The method according to claim 1, characterized in that, The second position observation equation is: ； in, For the node to be located in the th k The observation position of the node at any given moment; For the node to be located in the th k The Jacobian matrix of the position observation at time _____. This is the coordinate system transformation matrix. For the node to be located in the th k The position error matrix corresponding to the given time.

4. The method according to claim 1, 2 or 3, characterized in that, The second position observation equation includes the position error matrix corresponding to the node to be located; The method further includes: Based on the posterior estimated position error matrix of the previous time step, the prior estimated position error matrix of the current time step is determined; the prior estimated position error matrix of the current time step is the position error matrix of the node to be located at the current time step. Determine the gain coefficient at the current moment; The prior estimated position error matrix at the current time is updated based on the gain coefficient at the current time to obtain the posterior estimated position error matrix at the current time.

5. The method according to claim 4, characterized in that, The step of obtaining the node position of the node to be located based on the reference position, the node distance, the relative displacement parameter, and the second position observation equation includes: Substituting the reference position, the node distance, and the relative displacement parameter into the first position observation equation, the three-dimensional position closed-form solution of the node to be located is obtained; The posterior estimated position error matrix at the current time, calculated based on the second position observation equation, is used to correct the three-dimensional position closed-form solution, thereby obtaining the node position of the node to be located.

6. The method according to claim 5, characterized in that, The step of updating the prior estimated position error matrix at the current time based on the gain coefficient at the current time to obtain the posterior estimated position error matrix at the current time includes: After determining the closed-form solution of the three-dimensional position of the node to be located at the current time, the position measurement residual between the closed-form solution of the three-dimensional position at the current time and the theoretical observation position at the current time is determined. Based on the gain coefficient and position measurement residual at the current moment, the prior estimated position error matrix at the current moment is updated to obtain the posterior estimated position error matrix at the current moment.

7. The method according to claim 4, characterized in that, Determining the gain coefficient at the current moment includes: Based on the posterior estimated covariance of the previous time step, determine the prior estimated covariance of the current time step; Based on the prior estimated covariance at the current moment, determine the gain coefficient at the current moment; The method further includes: The prior estimated covariance at the current time is updated based on the gain coefficient at the current time to obtain the posterior estimated covariance at the current time.

8. A node positioning device, characterized in that, The device includes: A reference position module is used to determine the inertial measurement data of the node to be located when it is in a denied environment, and to determine the reference position of an anchor node located outside the denied environment; the anchor node is an unmanned device. The node distance module is used to measure the original distance between the anchor node and the node to be located using the time-of-flight positioning method; the original distance is corrected using a first-order Markov model to determine the node distance between the anchor node and the node to be located. The displacement parameter module is used to determine the relative displacement parameters of the node to be located based on the inertial measurement data. The position determination module is used to determine a first position observation equation for the node to be located, and to perform a linearized Taylor expansion on the first position observation equation to determine a second position observation equation. The first position observation equation is constructed under the constraint of the relative displacement parameter of the node to be located, based on the position of the anchor node and the distance between the anchor node and the node to be located. The node position of the node to be located is obtained based on the reference position, the node distance, the relative displacement parameter, and the second position observation equation.

9. An electronic device, characterized in that, include: A memory and a processor are communicatively connected, the memory stores computer instructions, and the processor executes the node positioning method of any one of claims 1 to 7 by executing the computer instructions.

Images