CN109959918B - Solid body positioning method and device and computer storage medium - Google Patents

Abstract

Embodiments of the present invention disclose a method, a device, and a computer storage medium for locating a solid-state body; the method may include: determining direction information and position information of the target solid-state body based on sensors distributed on the target solid-state body; Noise covariance information and measurement vector of anchor nodes around the target solid body; wherein, the anchor nodes include anchor nodes with position errors; according to the direction information and position information of the target solid body and the anchor nodes The noise covariance information and the measurement vector construct an estimator, and obtain the log-likelihood function of the estimator; based on the log-likelihood function of the estimator, obtain the estimator according to the set maximum likelihood estimation algorithm strategy. maximum likelihood solution for the estimator.

Description

A method, device and computer storage medium for positioning a solid state

technical field

The present invention relates to the technical field of signal processing, and in particular, to a method, a device and a computer storage medium for positioning a solid state.

Background technique

Active positioning technology has always been the focus of attention in the industry because of its effectiveness. This technology includes sonar positioning, radar positioning, microphone array positioning and the current wireless sensor network (WSNs, Wireless Sensors Networks) positioning technology. Traditional active localization techniques usually treat the target of interest as a point source, so that the position of the target can be defined by a three-dimensional (3D) single-point position or a two-dimensional (2D) position area. However, in many positioning applications, the target of interest needs to be treated as a solid body with zero or negligible deformation; and on a solid body, the distance between any given two points is non-negligible So there is a solid body localization (RBL, Rigid BodyLocalization) technology.

In conventional RBL technology, a stationary solid body can be precisely positioned using a time or distance measurement method between a sensor placed in the solid body and some surrounding landmark information (also called anchor nodes); therefore, a solid state The precise location estimation of the body is very dependent on the location accuracy of the anchor nodes. However, in general, with the position error of the anchor node, the positioning accuracy of the solid body will be reduced. Therefore, it is necessary to accurately estimate the position of the solid body for the anchor node with error.

SUMMARY OF THE INVENTION

In view of this, the embodiments of the present invention are expected to provide a method, an apparatus, and a computer storage medium for positioning a solid body; in the case of an error in the position of an anchor node, the position of the solid body can be accurately estimated.

The technical scheme of the present invention is realized as follows:

In a first aspect, an embodiment of the present invention provides a method for positioning a solid body, the method comprising:

Determine the direction information and position information of the target solid body based on the sensors distributed on the target solid body;

Acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes include anchor nodes with position errors;

An estimator is constructed according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and the log-likelihood function of the estimator is obtained; wherein, the estimator is Including the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node;

Based on the log-likelihood function of the estimator, obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy; wherein, the maximum likelihood solution of the estimator includes the target solid body The direction estimate and position estimate of , and the position estimate of the anchor node.

In a second aspect, an embodiment of the present invention provides an apparatus for positioning a solid body, the apparatus includes: a determination part, an acquisition part, a construction part and an estimation part; wherein,

The determining part is configured to determine the direction information and position information of the target solid body based on the sensors distributed on the target solid body;

The acquisition part is configured to acquire noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein, the anchor nodes include anchor nodes with position errors;

The construction part is configured to construct an estimator according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and obtain a log-likelihood function of the estimator ; Wherein, the estimator includes the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node;

The estimation part is configured to obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy based on the log-likelihood function of the estimator; wherein, the maximum likelihood of the estimator is The solution includes the orientation estimate and position estimate of the target solid body, and the position estimate of the anchor node.

In a third aspect, an embodiment of the present invention provides an apparatus for positioning a solid-state body, the apparatus includes: a communication interface, a memory, and a processor; wherein, the communication interface is used for sending and receiving with other external network elements In the information process, the reception and transmission of signals;

the memory for storing a computer program executable on the processor;

The processor is configured to, when running the computer program, execute the steps of the method for locating the solid state body of the first aspect.

In a fourth aspect, an embodiment of the present invention provides a computer storage medium, characterized in that the computer storage medium stores a program for locating a solid-state body, and the program for locating a solid-state body when executed by at least one processor implements the first The method steps of solid body localization of the aspect.

The embodiments of the present invention provide a method, a device and a computer storage medium for locating a solid body; the maximum likelihood estimation algorithm is used to jointly estimate the direction and position of the target solid body and the position of an anchor node with a position error, which not only solves the problem of Nonlinear Constrained Optimization Problems Existing in Joint Estimation. Moreover, the position of the solid body can be accurately estimated in the case of errors in the position of the anchor node.

Description of drawings

FIG. 1 is a schematic flowchart of a method for positioning a solid body according to an embodiment of the present invention;

FIG. 2 is a schematic diagram of the correction performance of the anchor node position A proposed by an embodiment of the present invention;

3 is a schematic diagram of the influence of the estimation accuracy of the rotation angle q proposed by an embodiment of the present invention;

4 is a schematic diagram of the influence of the estimation accuracy of the translation vector t proposed in an embodiment of the present invention;

5 is a schematic diagram of an estimation effect of the anchor node position A proposed by an embodiment of the present invention;

6 is a schematic diagram of an estimation effect of a rotation angle q proposed by an embodiment of the present invention;

7 is a schematic diagram of an estimation effect of the translation vector t proposed by an embodiment of the present invention;

8 is a schematic diagram of the composition of a solid-state body positioning device proposed in an embodiment of the present invention;

9 is a schematic diagram of the composition of another solid-state body positioning device proposed in an embodiment of the present invention;

FIG. 10 is a schematic diagram of a specific hardware structure of a solid-state body positioning device according to an embodiment of the present invention.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

Solid state localization can now be widely used in driverless cars, robotic systems, underwater vehicles, ships, drones, orbiting satellites, spacecraft, and many other applications where it is not only necessary to estimate the Absolute position, but also its orientation needs to be precisely determined. In fields such as aerospace, marine heading, driverless car orientation and robotic systems or industrial equipment tilt and consumer equipment, determining the orientation of a solid body can be called direction finding. Taking a driverless car as an example, the driverless car as a solid body not only moves but also turns, so it is necessary to determine the direction of the solid body for basic control, manipulation and safety assurance. In conventional approaches to localization of solid bodies, although position and orientation are closely related, they are still treated separately.

For example, some conventional solutions are to install several sensors on a solid body, and use distance measurement (or time measurement) for the wireless sensor network (in this embodiment of the present invention, it may be composed of some landmarks simply called anchor nodes). ) together to determine its orientation and location. When manufacturing a solid body, although the absolute position of the solid body is unknown, the topology of the sensors mounted on the solid body or the relative position of the sensors can be measured, so the orientation and absolute position of the solid body can be estimated. For solid bodies, the orientation of the solid body is represented as a rotation matrix (or rotation angle vector), and the absolute position of the solid body is represented as a translation vector. The joint estimation of the rotation matrix (or rotation angle vector) and translation vector can be considered as a nonlinear constrained optimization problem, because the rotation matrix (or rotation angle vector) and translation vector are nonlinearly related to the distance measurement, and they are related to each other. are closely linked. Furthermore, the rotation matrix is not a free variable, it must belong to a special orthogonal group, which means that its elements must satisfy the square constraint in the two-dimensional case and the cubic constraint in the three-dimensional case. In view of the above problems, the embodiments of the present invention are expected to be able to solve the problem through maximum likelihood estimation (MLE, Maximum Likelihood Estimate).

Based on this, referring to FIG. 1 , which shows a method for positioning a solid body provided by an embodiment of the present invention, which may include:

S101: Determine direction information and position information of the target solid body based on sensors distributed on the target solid body;

S102: Acquire noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein, the anchor nodes include anchor nodes with position errors;

S103: Construct an estimator according to the direction information and position information of the target solid body, the noise covariance information of the anchor node, and the measurement vector, and obtain a log-likelihood function of the estimator; wherein, the The estimator includes the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node;

S104: Based on the log-likelihood function of the estimator, obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy; wherein, the maximum likelihood solution of the estimator includes the target The orientation estimate and position estimate of the solid body, and the position estimate of the anchor node.

From the technical solution shown in FIG. 1, the embodiment of the present invention performs joint estimation on the direction and position of the target solid body and the position of the anchor node with the position error through the maximum likelihood estimation algorithm, which not only solves the problems existing in the joint estimation. Linear Constrained Optimization Problem. Moreover, the position of the solid body can be accurately estimated in the case of errors in the position of the anchor node.

For the technical solution shown in FIG. 1 , in a possible implementation manner, the determining of the direction information and position information of the target solid body based on sensors distributed on the target solid body includes:

其中，n表示传感器标识，n＝1,2,…,N；Corresponding to the number of the sensors being N and the relative positions of the sensors being known, the position information of the sensors in the local reference frame is

Among them, n represents the sensor identification, n=1,2,...,N;

其中，

Q和t分别为由局部参考系到全局参考系的旋转矩阵和平移矢量，且用于表示所述目标固态体的方向信息和位置信息。Based on the position information of each of the sensors in the local reference frame, determine the position information of each of the sensors in the global reference frame

in,

Q and t are the rotation matrix and translation vector from the local reference frame to the global reference frame, respectively, and are used to represent the orientation information and position information of the target solid body.

For the technical solution shown in FIG. 1, in a possible implementation manner, the acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body includes:

Corresponding to the number of anchor nodes being M, the location information of the anchor nodes in the global reference frame is a _m , where m represents an anchor node identifier, and m=1, 2,...,M;

Generate the position matrix of the anchor node according to the position information of the anchor node

其中，所述σ_a为所述锚节点的位置误差的标准差σ_a(m)；Corresponding to the Gaussian random process with zero mean and independent and identical distribution of the position error of the anchor node, the noise covariance matrix of the anchor node

Wherein, the σ _a is the standard deviation σ _a (m) of the position error of the anchor node;

为高斯分布且所述测量向量的协方差矩阵为R_r,n，所有测量向量

为高斯分布且所有测量向量的协方差矩阵为R_r＝Bdiag(R_r,1,R_r,2,…,R_r,N)。corresponding to the measurement vector

is Gaussian distributed and the covariance matrix of the measurement vectors is R _r,n , all measurement vectors

is Gaussian distributed and the covariance matrix of all measurement vectors is R _r =Bdiag(R _r,1 ,R _r,2 ,...,R _r,N ).

对于测量向量来说，测量向量的数量与传感器数量一致，并且每个测量向量内的测量元素数量与锚节点数量相同，而且刚体传感器阵列通常较小，距离锚节点较远，因此不同传感器的距离测量噪声功率是相同的，也就是说R_r,1＝R_r,2＝…＝R_r,N。It should be noted that the above implementation manner can be used to set the positioning scene of the target solid body. Specifically, when setting the positioning scene, N sensor nodes can be distributed on the static target solid body, and M anchor nodes with position error can be distributed around the target solid body, and the anchor node position error can be regarded as a Gaussian random process with zero mean independent and identical distribution, and the standard deviation of the position error of each anchor node σ _a (m) = σ _a , whose covariance matrix is

For measurement vectors, the number of measurement vectors is the same as the number of sensors, and the number of measurement elements in each measurement vector is the same as the number of anchor nodes, and the rigid body sensor array is usually smaller and farther away from the anchor nodes, so the distance of different sensors The measured noise power is the same, ie R _r,1 =R _r,2 =...=R _r,N .

For the foregoing technical solution, in a possible implementation manner, the estimator is constructed according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and the obtained log-likelihood functions of the estimators, including:

其中，

q^o和t^o分别表示所述目标固态体的旋转角度估计量和平移矢量估计量，A^o表示所述锚节点的位置信息估计量，所述目标固态体的旋转角度q与所述目标固态体旋转矩阵Q具有以下关系：generate estimators

in,

q ^o and t ^o represent the rotation angle estimate and translation vector estimate of the target solid body, respectively, A ^o represents the position information estimate of the anchor node, and the rotation angle q of the target solid body is related to the target solid body. The body rotation matrix Q has the following relationship:

其中，所述目标固态体旋转矩阵Q为特殊正交群

q=[α,β,γ] ^T , Q=Q _γ Q _β Q _α ,

Wherein, the rotation matrix Q of the target solid body is a special orthogonal group

The log-likelihood function of the estimator determined according to the estimator ψ ^o is:

where n is a constant,

For the foregoing technical solution, in a possible implementation manner, the maximum likelihood solution of the estimator is obtained according to the set maximum likelihood estimation algorithm strategy based on the log-likelihood function of the estimator, including :

According to the maximum likelihood rule, the corresponding least squares model is generated according to the log-likelihood function lnp(ζ,ψ ^o ) of the estimator as follows:

其中，Set the initial value ζ ^o (ψ)=ζ ^o (ψ ^{0} )+G ^{0} (ψ-ψ ^{0} ), where G ^{0} is the gradient matrix and

in,

其中，

是Q^o相对于q^o的导数；

in,

is the derivative of Q ^o with respect to q ^o ;

Based on the initial value, according to the Gauss-Newton iteration method, the least squares model is iterated according to the set number of iterations and the iterative equation shown in the following formula to obtain the maximum likelihood solution of the estimator

ψ ^{k+1} = ψ ^{k} +(G ^{k}T R ^-1 G ^{k} ) ^-1 G ^{k}T R ^-1 (ζ-ζ ^o (ψ ^{k} )),k =0,1,...

where k represents the number of iterations.

It should be noted that the iterative solution obtained after iterating according to the set number of iterations will tend to converge, so as to meet the set accuracy requirements. At this time, the obtained iterative solution is considered to be the maximum likelihood of the estimator. Ran solution

Understandably, in the case where the exact position of the anchor node is not completely clear, by modeling the anchor node position error as additive Gaussian noise, it can be obtained from the Cramer-Rao Lower Bound (CRLB) analysis: The decrease of the positioning accuracy of the target solid body is caused by the position error of the anchor node; it can be known that in a more practical application scenario, the accurate position estimation of the target solid body is very dependent on the position accuracy of the anchor node. The technical solution and implementation shown in FIG. 1 above, based on the maximum likelihood estimation (MLE) method proposed for the position error of the anchor node, can be determined by comparing with the lower bound of Cramero. Technical effect of the proposed method for localization of solid bodies.

Based on this, for the technical solution and implementation manner shown in FIG. 1 above, in a possible implementation manner, the method further includes:

Determine the Cramero lower bound of the rotation angle estimator and the translation vector estimator of the target solid body according to the estimator and the log-likelihood function of the estimator;

Under various anchor node position error strengths, the mean square error of the maximum likelihood solution of the estimator is obtained, and compared with the lower bound of Cramero's position error strength of each anchor node.

Preferably, for the above implementation manner, the determining the Cramero lower bound of the rotation angle estimator and the translation vector estimator of the target solid body according to the estimator and the log-likelihood function of the estimator includes:

求二阶偏导数，获得费雪信息(Fisher Information)矩阵

Apply the log-likelihood function lnp(ζ,ψ ^o ) of the estimator to the estimator

Find the second-order partial derivative and obtain the Fisher Information matrix

其中，

是Q^o相对于q^o的导数；in,

in,

is the derivative of Q ^o with respect to q ^o ;

的克拉美罗下界

以及的A^o的克拉美罗下界CRLB(A^o)＝FIM(A^o)^-1＝(Z-Y^TX^-1Y)^-1。Based on the Fisher information matrix FIM(ψ ^o ), through the block matrix inversion formula, respectively obtain the estimator ψ ^o

Kramero Nether

And the Cramero lower bound of A ^o CRLB(A ^o )=FIM(A ^o ) ⁻¹ =(ZY ^T X ⁻¹ Y) ⁻¹ .

It should be noted that, by comparing the mean square error of the maximum likelihood solution of the estimator under various anchor node position error strengths with the Cramero lower bound of each anchor node position error strength, it can be intuitively known that the embodiment of the present invention has Technical effect of the proposed method for localization of solid bodies. The embodiments of the present invention illustrate the technical effects of the method for positioning a solid body proposed by the embodiments of the present invention through the following specific simulation scenarios.

First, set the simulation conditions as follows:

1. Based on the 3D positioning scene, when the position of the anchor node is uncertain, the number of anchor nodes is set to M = 6, which are evenly distributed in the three-dimensional cube centered on the origin O in the global reference system (±50m). ×±50m×±50m). In order to avoid the poor distribution structure affecting the positioning performance, the distance between two anchor nodes should be at least 15 meters. According to the above conditions, G=200 geometric distributions of M=6 anchor nodes can be randomly generated, and the simulation result is the average value thereof. For each given geometry, the number of simulation runs is L = 1000, and each sensor is able to obtain measurements from all anchor nodes.

在局部参考系中，C中每列代表传感器位置c_n。目标固态体的旋转设定如下：两个参考系在开始时重合，然后固态体旋转α＝20度，β＝-25度，γ＝10度。平移向量是t＝[100,100,50]^T。在整个仿真过程中，固态体传感器阵列通常很小并且远离锚节点。因此，可以将从给定锚节点到所有传感器的范围内的噪声功率设置为相同。但是对于一个给定的刚体传感器，为了更好地运用算法性能，可以将从刚体传感器到不同锚点范围的噪声功率设置为不同。2. The target solid body settings and sensor configuration are specified as follows: For a 3D RBL scene, the true position of the rigid body sensor is

In the local reference frame, each column in C represents the sensor position _cn . The rotation of the target solid body is set as follows: the two reference frames coincide at the beginning, and then the solid body is rotated by α = 20 degrees, β = -25 degrees, and γ = 10 degrees. The translation vector is t=[100, 100, 50] ^T . Throughout the simulation, the solid-state body sensor arrays are typically small and far away from the anchor nodes. Therefore, the noise power in the range from a given anchor node to all sensors can be set to be the same. But for a given rigid body sensor, in order to better utilize the algorithm performance, the noise power can be set to be different from the rigid body sensor to the range of different anchor points.

其中，

为克罗内克积。在后续仿真中，3D场景下距离测量噪声功率

固定在-40dB。加入协方差矩阵为

的零均值高斯噪声的真实值来构建有位置误差的锚节点位置。噪声协方差矩阵的设置用于本仿真中的所有数值实例和仿真。Therefore, for a number of anchor nodes M = 6, the covariance matrix of the distance measurement noise is

in,

is the Kronecker product. In subsequent simulations, the distance measurement noise power in the 3D scene

Fixed at -40dB. Join the covariance matrix as

The ground truth value of zero-mean Gaussian noise to construct the anchor node position with position error. The setup of the noise covariance matrix is used for all numerical examples and simulations in this simulation.

Next, simulation is performed based on the above-mentioned simulation conditions and the above-mentioned method for locating the solid body. For the specific process, refer to the technical solution and each implementation manner shown in the above-mentioned FIG. 1 , which will not be repeated here. The specific simulation results and analysis are as follows:

1. CRLB analysis

的数值，可以利用提出的CRLB结果验证锚节点位置A对固态体定位的影响。对于3D场景，CRLB仿真结果如图2至图4所示。其中，图2示出了锚节点位置A的修正性能，图3示出了旋转角度q的估计精度影响情况，图4中示出了平移矢量t的估计精度影响情况。在图3和图4中，锚节点位置误差ΔA不存在时参数估计的CRLB也被绘制用于比较。By changing the noise power of the anchor node location

, the effect of anchor node position A on solid body localization can be verified using the proposed CRLB results. For the 3D scene, the CRLB simulation results are shown in Figures 2 to 4. Among them, Fig. 2 shows the correction performance of the anchor node position A, Fig. 3 shows the influence of the estimation accuracy of the rotation angle q, and Fig. 4 shows the influence of the estimation accuracy of the translation vector t. In Figures 3 and 4, the CRLB of the parameter estimates when the anchor node position error ΔA is absent is also plotted for comparison.

时，锚节点位置不确定性ΔA可以明显地被校正。For Figure 2, the abscissa is the noise power of the anchor node position (Anchor Position Noise), the ordinate is the root value of the CRLB of the anchor node position A, the dotted line represents the CRLB boundary without position correction, and the square dotted line Represents the CRLB bound with position correction. It can be seen that after the correction using the distance measurement vector r, when the noise intensity of the anchor node position is relatively large, the CRLB bound estimated by the anchor node position A is significantly smaller than that without any correction. , which also proves that when the measurement vector r is used to estimate the position vector of the target solid body

, the anchor node position uncertainty ΔA can be significantly corrected.

For Figures 3 and 4, the abscissa is the noise power of the anchor node position (Anchor PositionNoise), while the ordinate of Figure 3 is the root value of the CRLB of the rotation angle q of the solid body, and the ordinate of Figure 4 represents the solid state. The root value of the CRLB of the volume's translation vector t.

固定在较小的水平，由锚节点位置误差ΔA引起的q和t估计误差在实际场景的应用中不可忽略。The meter-shaped dot-dash line represents the CRLB bound of the parameter estimation when the anchor node position error ΔA does not exist, and the square dotted line represents the CRLB bound when the anchor node position error ΔA exists. Below, the estimated CRLBs of q and t are clearly determined by the anchor node position, which proves that the anchor node position uncertainty ΔA significantly deteriorates the estimation performance of q and t. This means that even distance measurement noise power

Fixed at a small level, the q and t estimation errors caused by the anchor node position error ΔA are not negligible in practical application.

2. Analysis of Root Mean Squared Error (RMSE, Root Mean Squared Error)

Fig. 5 shows the estimation effect of the anchor node position A of the solution proposed by the embodiment of the present invention, and Fig. 6 and Fig. 7 show the estimation effect of the rotation angle q and the translation vector t respectively. The CRLB of the parameter estimates in the absence of the node position error ΔA is also plotted for comparison.

从10^-4增加到10¹的过程中，本发明实施例所提出的固态体定位方法对锚节点位置A估计的均方误差(RMSE)能够很好地实现其相应的CRLB精度。For FIG. 5 , the abscissa is the noise power of the anchor node position (Anchor Position Noise), the ordinate is the RMSE of the anchor node position A, and the meter-shaped dot-dash line represents the use of the solid-state body positioning method proposed by the embodiment of the present invention. After the RMSE of the estimated value obtained, the square dotted line indicates that the anchor node has the CRLB bound under the condition of position error, it can be seen that when the anchor node position noise power

During the process of increasing from 10 ⁻⁴ to 10 ¹ , the mean square error (RMSE) of the anchor node position A estimated by the solid body localization method proposed in the embodiment of the present invention can well achieve its corresponding CRLB accuracy.

For Fig. 6 and Fig. 7, the abscissa is the noise power of the anchor node position (Anchor PositionNoise), and the ordinate of Fig. 6 is the RMSE of the rotation angle q of the solid body, and the ordinate of Fig. 7 represents the translation of the solid body The RMSE of the vector t.

The meter-shaped dot-dash line represents the RMSE of the estimated value obtained by using the solid body localization method proposed in the embodiment of the present invention, the square dotted line represents the CRLB boundary when the anchor node position error ΔA exists, and the circular dotted solid line represents the absence of CRLB bound at anchor node position error ΔA. It can be seen from this that when the noise power of the anchor node position is low, the solid body localization method proposed in the embodiment of the present invention can well achieve the corresponding mean square error (RMSE) of the translation vector t and the rotation angle q estimation. When the noise power of the anchor node position reaches 10 ¹ , the mean square error (RMSE) of the rotation angle q estimation is slightly higher than its Cramero bound, while the mean square error (MSE) of the translation vector t estimation is significantly higher than Its Cramero realm. It can be known from this that the influence of the anchor node position error ΔA on the translation vector t is more serious than that of the rotation angle q, so the influence of the anchor node error must be considered in practical applications.

Based on the same inventive concept as the foregoing technical solutions, see FIG. 8 , which shows a device 80 for positioning a solid body provided by an embodiment of the present invention. The device 80 may include: a determination part 801 , an acquisition part 802 , and a construction part 803 and estimation section 804; where,

The determining part 801 is configured to determine the direction information and position information of the target solid body based on the sensors distributed on the target solid body;

The acquisition part 802 is configured to acquire noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein, the anchor nodes include anchor nodes with position errors;

The construction part 803 is configured to construct an estimator according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and obtain the log-likelihood of the estimator function; wherein, the estimator includes the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node;

The estimating part 804 is configured to obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy based on the log-likelihood function of the estimator; wherein, the maximum likelihood solution of the estimator is However, the solution includes the estimated direction value and the estimated value of the position of the target solid body, and the estimated value of the position of the anchor node.

In the above solution, the determining part 801 is configured as:

其中，n表示传感器标识，n＝1,2,…,N；Corresponding to the number of the sensors being N and the relative positions of the sensors being known, the position information of the sensors in the local reference frame is

Among them, n represents the sensor identification, n=1,2,...,N;

其中，

Q和t分别为由局部参考系到全局参考系的旋转矩阵和平移矢量，且用于表示所述目标固态体的方向信息和位置信息。Based on the position information of each of the sensors in the local reference frame, determine the position information of each of the sensors in the global reference frame

in,

Q and t are the rotation matrix and translation vector from the local reference frame to the global reference frame, respectively, and are used to represent the orientation information and position information of the target solid body.

In the above solution, the acquisition part 802 is configured as:

Corresponding to the number of anchor nodes being M, the location information of the anchor nodes in the global reference frame is a _m , where m represents an anchor node identifier, and m=1, 2,...,M;

Generate the position matrix of the anchor node according to the position information of the anchor node

其中，所述σ_a为所述锚节点的位置误差的标准差σ_a(m)；Corresponding to the Gaussian random process with zero mean and independent and identical distribution of the position error of the anchor node, the noise covariance matrix of the anchor node

Wherein, the σ _a is the standard deviation σ _a (m) of the position error of the anchor node;

为高斯分布且所述测量向量的协方差矩阵为R_r,n，所有测量向量

为高斯分布且所有测量向量的协方差矩阵为R_r＝Bdiag(R_r,1,R_r,2,…,R_r,N)。corresponding to the measurement vector

is Gaussian distributed and the covariance matrix of the measurement vectors is R _r,n , all measurement vectors

is Gaussian distributed and the covariance matrix of all measurement vectors is R _r =Bdiag(R _r,1 ,R _r,2 ,...,R _r,N ).

In the above solution, the construction part 803 is configured as:

其中，

q^o和t^o分别表示所述目标固态体的旋转角度估计量和平移矢量估计量，A^o表示所述锚节点的位置信息估计量，所述目标固态体的旋转角度q与所述目标固态体旋转矩阵Q具有以下关系：generate estimators

in,

q ^o and t ^o represent the rotation angle estimate and translation vector estimate of the target solid body, respectively, A ^o represents the position information estimate of the anchor node, and the rotation angle q of the target solid body is related to the target solid body. The body rotation matrix Q has the following relationship:

其中，所述目标固态体旋转矩阵Q为特殊正交群

q=[α,β,γ] ^T , Q=Q _γ Q _β Q _α ,

Wherein, the rotation matrix Q of the target solid body is a special orthogonal group

The log-likelihood function of the estimator determined according to the estimator ψ ^o is:

where n is a constant,

In the above solution, the estimation part 804 is configured as:

According to the maximum likelihood rule, the corresponding least squares model is generated according to the log-likelihood function lnp(ζ,ψ ^o ) of the estimator as follows:

其中R＝Bdiag(R_r,R_A)；

where R=Bdiag(R _r , R _A );

其中，Set the initial value ζ ^o (ψ)=ζ ^o (ψ ^{0} )+G ^{0} (ψ-ψ ^{0} ), where G ^{0} is the gradient matrix and

in,

其中，

是Q^o相对于q^o的导数；

in,

is the derivative of Q ^o with respect to q ^o ;

Based on the initial value, according to the Gauss-Newton iteration method, the least squares model is iterated according to the set number of iterations and the iterative equation shown in the following formula to obtain the maximum likelihood solution of the estimator

ψ ^{k+1} = ψ ^{k} +(G ^{k}T R ^-1 G ^{k} ) ^-1 G ^{k}T R ^-1 (ζ-ζ ^o (ψ ^{k} )),k =0,1,...

where k represents the number of iterations.

Referring to FIG. 9, the apparatus 80 further includes: a CRLB generation part 805 and a comparison part 806; wherein, the CRLB generation part 805 is configured to determine according to the estimator and the log-likelihood function of the estimator The Cramero lower bound of the rotation angle estimator and the translation vector estimator of the target solid body;

The comparison part 806 is configured to obtain the mean square error of the maximum likelihood solution of the estimator under various anchor node position error strengths, and compare it with the lower bound of Cramero's position error strength of each anchor node.

In the above solution, the CRLB generation part 805 is configured as:

求二阶偏导数，获得费雪信息矩阵

Apply the log-likelihood function lnp(ζ,ψ ^o ) of the estimator to the estimator

Find the second-order partial derivative and get the Fisher information matrix

其中，

是Q^o相对于q^o的导数；in,

in,

is the derivative of Q ^o with respect to q ^o ;

的克拉美罗下界

以及的A^o的克拉美罗下界CRLB(A^o)＝FIM(A^o)^-1＝(Z-Y^TX^-1Y)^-1。Based on the Fisher information matrix FIM(ψ ^o ), through the block matrix inversion formula, respectively obtain the estimator ψ ^o

Kramero Nether

And the Cramero lower bound of A ^o CRLB(A ^o )=FIM(A ^o ) ⁻¹ =(ZY ^T X ⁻¹ Y) ⁻¹ .

It can be understood that, in this embodiment, a "part" may be a part of a circuit, a part of a processor, a part of a program or software, etc., of course, it may also be a unit, or a module or non-modularity.

In addition, each component in this embodiment may be integrated into one processing unit, or each unit may exist physically alone, or two or more units may be integrated into one unit. The above-mentioned integrated units can be implemented in the form of hardware, or can be implemented in the form of software function modules.

If the integrated unit is implemented in the form of a software functional module and is not sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this embodiment is essentially or The part that contributes to the prior art or the whole or part of the technical solution can be embodied in the form of a software product, the computer software product is stored in a storage medium, and includes several instructions for making a computer device (which can be It is a personal computer, a server, or a network device, etc.) or a processor (processor) that executes all or part of the steps of the method described in this embodiment. The aforementioned storage medium includes: U disk, removable hard disk, Read Only Memory (ROM, Read Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes.

Therefore, this embodiment provides a computer storage medium, where the computer storage medium stores a solid-state body positioning program, and when the solid-state body positioning program is executed by at least one processor, implements the solid-state body positioning in the above technical solution steps of the method.

Based on the above-mentioned apparatus 80 for positioning a solid-state body and a computer storage medium, see FIG. 10 , which shows a specific hardware structure of a device 80 for positioning a solid-state body provided by an embodiment of the present invention, including: a communication interface 1001 , a memory 1002 and a processing 1003; the various components are coupled together through a bus system 1004. It will be appreciated that the bus system 1004 is used to implement connection communication between these components. In addition to the data bus, the bus system 1004 also includes a power bus, a control bus, and a status signal bus. However, for clarity of illustration, the various buses are labeled as bus system 1004 in FIG. 10 . in,

The communication interface 1001 is used for receiving and sending signals in the process of sending and receiving information with other external network elements;

the memory 1002 for storing computer programs that can run on the processor 1003;

The processor 1003 is configured to execute the following steps when running the computer program:

Determine orientation information and position information of the target solid body based on sensors distributed on the target solid body; and,

Acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes include anchor nodes with position errors; and,

An estimator is constructed according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and the log-likelihood function of the estimator is obtained; wherein, the estimator is including the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node; and,

Based on the log-likelihood function of the estimator, obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy; wherein, the maximum likelihood solution of the estimator includes the target solid body The direction estimate and position estimate of , and the position estimate of the anchor node.

It can be understood that the memory 1002 in the embodiment of the present invention may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memory. Wherein, the non-volatile memory may be Read-Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (Erasable PROM, EPROM), Erase programmable read-only memory (Electrically EPROM, EEPROM) or flash memory. The volatile memory may be random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as Static RAM (SRAM), Dynamic RAM (DRAM), Synchronous DRAM, SDRAM), double data rate synchronous dynamic random access memory (Double DataRate SDRAM, DDRSDRAM), enhanced synchronous dynamic random access memory (Enhanced SDRAM, ESDRAM), synchronous link dynamic random access memory (Synchlink DRAM, SLDRAM) and Direct memory bus random access memory (Direct Rambus RAM, DRRAM). The memory 1002 of the systems and methods described herein is intended to include, but not be limited to, these and any other suitable types of memory.

The processor 1003 may be an integrated circuit chip with signal processing capability. In the implementation process, each step of the above-mentioned method can be completed by an integrated logic circuit of hardware in the processor 1003 or an instruction in the form of software. The aforementioned processor 1003 may be a general-purpose processor, a digital signal processor (Digital Signal Processor, DSP), an application specific integrated circuit (ASIC), a field programmable gate array (Field Programmable Gate Array, FPGA), or other Programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. Various methods, steps, and logical block diagrams disclosed in the embodiments of the present invention can be implemented or executed. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The steps of the method disclosed in conjunction with the embodiments of the present invention may be directly embodied as executed by a hardware decoding processor, or executed by a combination of hardware and software modules in the decoding processor. The software modules may be located in random access memory, flash memory, read-only memory, programmable read-only memory or electrically erasable programmable memory, registers and other storage media mature in the art. The storage medium is located in the memory 1002, and the processor 1003 reads the information in the memory 1002, and completes the steps of the above method in combination with its hardware.

It will be appreciated that the embodiments described herein may be implemented in hardware, software, firmware, middleware, microcode, or a combination thereof. For hardware implementation, the processing unit may be implemented in one or more Application Specific Integrated Circuits (ASIC), Digital Signal Processing (DSP), Digital Signal Processing Device (DSP Device, DSPD), programmable logic Devices (Programmable Logic Device, PLD), Field-Programmable Gate Array (Field-Programmable Gate Array, FPGA), general purpose processors, controllers, microcontrollers, microprocessors, other electronic units for performing the functions described in this application or a combination thereof.

For a software implementation, the techniques described herein may be implemented through modules (eg, procedures, functions, etc.) that perform the functions described herein. Software codes may be stored in memory and executed by a processor. The memory can be implemented in the processor or external to the processor.

Specifically, the processor 1003 is further configured to execute the method steps of the solid-state body positioning described in the foregoing technical solutions when running the computer program, which will not be repeated here.

It should be noted that the technical solutions described in the embodiments of the present invention may be combined arbitrarily unless there is a conflict.

The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention. should be included within the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims (9)

1. A method for positioning a solid body, wherein the method comprises: Determine the direction information and position information of the target solid body based on the sensors distributed on the target solid body; Acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein the anchor nodes include anchor nodes with position errors; An estimator is constructed according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and the log-likelihood function of the estimator is obtained; wherein, the estimator is Including the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node; Based on the log-likelihood function of the estimator, obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy; wherein, the maximum likelihood solution of the estimator includes the target solid body The direction estimate value and position estimate value of , and the position estimate value of the anchor node; Wherein, obtaining the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy based on the log-likelihood function of the estimator, including: According to the maximum likelihood rule, the corresponding least squares model is generated according to the log-likelihood function ln p(ζ,ψ) of the estimator as follows:

where R=Bdiag(R _r , R _A ),

Wherein, R _r represents the covariance matrix of all measurement vectors; R _A represents the noise covariance matrix of the anchor node; r represents all the measurement vectors; A represents the anchor node position matrix; Set the initial value ζ ^o (ψ)=ζ ^o (ψ ^{0} )+G ^{0} (ψ-ψ ^{0} ), where G ^{0} is the gradient matrix and

in,

in,

is the derivative of Q ^o with respect to q ^o ; Wherein, M represents the number of the anchor nodes; N represents the number of the sensors;

Represents the position information of the Nth sensor in the global reference frame; m represents the anchor node identifier, m=1,2,...,M; n represents the sensor identifier, n=1,2,...,N ; q ^o represents the rotation angle estimator of the target solid body; Based on the initial value, according to the Gauss-Newton iteration method, the least squares model is iterated according to the set number of iterations and the iterative equation shown in the following formula to obtain the maximum likelihood solution of the estimator

ψ ^{k+1} = ψ ^{k} +(G ^{k}T R ^-1 G ^{k} ) ^-1 G ^{k}T R ^-1 (ζ-ζ ^o (ψ ^{k} )),k =0,1,... where k represents the number of iterations. 2 . The method according to claim 1 , wherein the determining the direction information and position information of the target solid body based on the sensors distributed on the target solid body comprises: 2 . Corresponding to the number of the sensors being N and the relative positions of the sensors being known, the position information of the sensors in the local reference frame is

Among them, n represents the sensor identification, n=1,2,...,N; Based on the position information of each of the sensors in the local reference frame, determine the position information of each of the sensors in the global reference frame

in,

Q and t are the rotation matrix and translation vector from the local reference frame to the global reference frame, respectively, and are used to represent the orientation information and position information of the target solid body. 3. The method according to claim 1, wherein the acquiring noise covariance information and measurement vectors of anchor nodes distributed around the target solid body comprises: Corresponding to the number of anchor nodes being M, the location information of the anchor nodes in the global reference frame is a _m , where m represents an anchor node identifier, and m=1, 2,...,M; Generate the position matrix of the anchor node according to the position information of the anchor node

Corresponding to the Gaussian random process with zero mean and independent and identical distribution of the position error of the anchor node, the noise covariance matrix of the anchor node

Wherein, the σ _a is the standard deviation σ _a (m) of the position error of the anchor node; corresponding to the measurement vector

is Gaussian distributed and the covariance matrix of the measurement vectors is R _r,n , all measurement vectors

is Gaussian distributed and the covariance matrix of all measurement vectors is R _r =Bdiag(R _r,1 ,R _r,2 ,...,R _r,N ). 4. The method according to any one of claims 1 to 3, wherein the structure is constructed according to the direction information and position information of the target solid body and the noise covariance information of the anchor node and the measurement vector estimator, and obtain the log-likelihood function of said estimator, including: generate estimators

in,

q ^o and t ^o represent the rotation angle estimate and translation vector estimate of the target solid body, respectively, A ^o represents the position information estimate of the anchor node, and the rotation angle q of the target solid body is related to the target solid body. The body rotation matrix Q has the following relationship: q=[α,β,γ] ^T , Q=Q _γ Q _β Q _α ,

Wherein, the rotation matrix Q of the target solid body is a special orthogonal group

The log-likelihood function of the estimator determined according to the estimator ψ is:

where n is a constant,

5. The method according to claim 1, wherein the method further comprises: Determine the Cramero lower bound of the rotation angle estimator and the translation vector estimator of the target solid body according to the estimator and the log-likelihood function of the estimator; Under various anchor node position error strengths, the mean square error of the maximum likelihood solution of the estimator is obtained, and compared with the lower bound of Cramero's position error strength of each anchor node. 6 . The method according to claim 5 , wherein, determining the difference between the rotation angle estimator and the translation vector estimator of the target solid body according to the estimator and the log-likelihood function of the estimator. 7 . The Kramero Nether, including: Apply the log-likelihood function lnp(ζ,ψ) of the estimator to the estimator

Find the second-order partial derivative and get the Fisher information matrix

in,

in,

is the derivative of Q ^o with respect to q ^o ; Based on the Fisher information matrix FIM(ψ), through the block matrix inversion formula, respectively obtain the estimator ψ in

Kramero Nether

And the Cramero lower bound of A ^o CRLB(A ^o )=FIM(A ^o ) ⁻¹ =(ZY ^T X− ¹ Y) ⁻¹ . 7. A device for positioning a solid body, characterized in that the device comprises: a determination part, an acquisition part, a construction part and an estimation part; wherein, The determining part is configured to determine the direction information and position information of the target solid body based on the sensors distributed on the target solid body; The acquisition part is configured to acquire noise covariance information and measurement vectors of anchor nodes distributed around the target solid body; wherein, the anchor nodes include anchor nodes with position errors; The construction part is configured to construct an estimator according to the direction information and position information of the target solid body, the noise covariance information of the anchor node and the measurement vector, and obtain a log-likelihood function of the estimator ; Wherein, the estimator includes the direction estimator and the position estimator of the target solid body, and the position estimator of the anchor node; The estimation part is configured to obtain the maximum likelihood solution of the estimator according to the set maximum likelihood estimation algorithm strategy based on the log-likelihood function of the estimator; wherein, the maximum likelihood of the estimator is The solution includes the estimated direction value and the estimated value of the position of the target solid body, and the estimated value of the position of the anchor node; Wherein, the estimation part is configured as: According to the maximum likelihood rule, the corresponding least squares model is generated according to the log-likelihood function lnp(ζ,ψ) of the estimator as follows:

where R=Bdiag(R _r , R _A ),

Wherein, R _r represents the covariance matrix of all measurement vectors; R _A represents the noise covariance matrix of the anchor node; r represents all the measurement vectors; A represents the anchor node position matrix; Set the initial value ζ ^o (ψ)=ζ ^o (ψ ^{0} )+G ^{0} (ψ-ψ ^{0} ), where G ^{0} is the gradient matrix and

in,

in,

is the derivative of Q ^o with respect to q ^o ; Wherein, M represents the number of the anchor nodes; N represents the number of the sensors;

Represents the position information of the Nth sensor in the global reference frame; m represents the anchor node identifier, m=1,2,...,M; n represents the sensor identifier, n=1,2,...,N ; q ^o represents the rotation angle estimator of the target solid body; Based on the initial value, according to the Gauss-Newton iteration method, the least squares model is iterated according to the set number of iterations and the iterative equation shown in the following formula to obtain the maximum likelihood solution of the estimator

ψ ^{k+1} = ψ ^{k} +(G ^{k}T R ^-1 G ^{k} ) ^-1 G ^{k}T R ^-1 (ζ-ζ ^o (ψ ^{k} )),k =0,1,... where k represents the number of iterations. 8. An apparatus for positioning a solid-state body, wherein the apparatus comprises: a communication interface, a memory and a processor; wherein, the communication interface is used for sending and receiving information with other external network elements, the reception and transmission of signals; the memory for storing a computer program executable on the processor; The processor is configured to execute the steps of the method for locating a solid state body according to any one of claims 1 to 6 when running the computer program. 9. A computer storage medium, characterized in that the computer storage medium stores a program for positioning a solid state body, and when the program for positioning the solid state body is executed by at least one processor, any one of claims 1 to 6 is implemented. The method steps for solid state localization are described.

Images