Calibration source based motion solid body positioning method

Abstract

The application provides a positioning method for a moving solid object based on a calibration source, and the implementation steps are as follows: (1) constructing a positioning scene of the moving solid object and initializing parameters; (2) constructing a measurement vector; (3) constructing an estimation vector and obtaining a maximum likelihood solution of the estimation vector; and (4) obtaining a positioning result of the moving solid object. The application introduces a calibration source in the positioning scene, uses distance measurement information between the calibration source and anchor nodes when constructing the measurement vector, and continuously eliminates the position error of the anchor nodes through the updating process of the measurement vector in each iteration process, so that the deterioration problem of the positioning precision caused by the position error of the anchor nodes is effectively inhibited, and the positioning precision of the moving solid object is improved.

Description

Technical Field

[0001] This invention belongs to the field of signal processing technology and relates to a solid-state positioning method, specifically a moving solid-state positioning method based on a calibration source, which can be applied to systems such as unmanned aerial vehicles, underwater rescue, etc. Background Technology

[0002] A solid body is an object that maintains the same shape and size during motion or under the action of external forces, and whose internal points maintain constant relative positions. An absolutely solid body does not actually exist; it is merely an ideal model. Because any object will deform under the application of external forces, if the degree of deformation is negligible relative to the object's geometric dimensions, the object can be considered a solid body. Based on their state of motion, solid bodies can be divided into stationary solid bodies and moving solid bodies. Positioning of a moving solid body refers to determining the position, orientation, angular velocity, and speed of a moving solid body using distance, time, or angle measurements between multiple sensors mounted on the solid body and multiple anchor nodes around it. For example, in autonomous vehicle applications, in addition to vehicle movement, there are also turning maneuvers. Therefore, besides the vehicle's position and orientation, it is also necessary to obtain the vehicle's angular velocity and speed while it is in motion to ensure basic control and real-time adjustments, thereby reducing operational risks. In underwater rescue systems, obtaining the position and orientation information of rescue equipment such as robots and drones ensures precise deployment of the equipment. At the same time, obtaining the angular velocity and speed information of the rescue equipment enables remote automatic operation, laying the foundation for the orderly conduct of rescue missions.

[0003] The key to positioning a moving solid body lies in improving positioning accuracy. The main factors affecting accuracy include the positional errors of anchor nodes around the solid body, the positional errors of sensors mounted on the solid body, the distance measurement errors between the sensors and anchor nodes, and Doppler measurement errors. For example, S. Chen and Ho, in their 2015 paper "Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks," in IEEE Transactions on Signal Processing, vol. 63, no. 24, pp. 6459-6472, Dec. 15, address these issues. In 2015, a method for locating a moving solid body with error elimination was proposed. This method first constructs a 3D positioning scene including a moving solid body equipped with multiple sensors and multiple stationary anchor nodes distributed around the moving solid body. The position information of all sensors is acquired in a local reference frame, and the position information of all anchor nodes and the error-laden parameter information of the solid body, including position, orientation, angular velocity, and velocity, are acquired in a global reference frame. Secondly, using the position information of all sensors in the local reference frame, the position information of all anchor nodes in the global reference frame, and the error-laden parameter information of the moving solid body, the position information of each anchor node is obtained. The distance measurement information and Doppler measurement information between the point and each sensor are used to construct a measurement vector. Then, a maximum likelihood model is constructed using the measurement vector, with the error-free position, orientation, angular velocity and velocity information of the moving solid body as the parameters to be estimated. Based on the Gauss-Newton iteration law, the distance measurement error and Doppler measurement error between the sensor and the anchor node are eliminated by updating the measurement vector in each iteration, thereby optimizing the maximum likelihood model. Finally, the maximum likelihood solution of the parameters to be estimated is obtained, and the estimation results of the position, orientation, angular velocity and velocity information of the moving solid body are obtained through mathematical transformation. This method effectively suppresses the deterioration of positioning accuracy caused by distance measurement errors between the sensor and anchor nodes and Doppler measurement errors. However, its shortcomings are that the positioning scenario constructed by this method is an ideal scenario in which all anchor nodes do not contain position errors. However, in practical applications, the position information of anchor nodes without errors is not available. Only the position information of anchor nodes with errors can be obtained. This will introduce additional errors into the measurement vector constructed based on the position information of all anchor nodes, causing the maximum likelihood solution obtained based on the measurement vector to deviate from the correct solution, ultimately resulting in a decrease in positioning accuracy. Summary of the Invention

[0004] The purpose of this invention is to overcome the defects of the prior art and propose a positioning method for moving solid bodies based on a calibration source, which solves the problem of decreased positioning accuracy caused by errors in the anchor node position in the prior art.

[0005] To achieve the above objectives, the technical solution adopted by the present invention includes the following steps:

[0006] (1) Construct the positioning scene of the moving solid body and initialize the parameters:

[0007] Construct a 3D positioning scene for the moving solid body, which includes N sensors, M stationary anchor nodes distributed around it, and a calibration source; initialize the position coordinates of the N sensors in a local reference frame Oq1q2q3 with the geometric center of the moving solid body as the origin as c = [c1, c2, ..., c]. n ,…,c N ] T The position coordinates of the M anchor nodes and calibration source in the global reference system Oxyz are respectively a = [a1, a2, ..., a...]. m ,…,a M ] T p = [p x ,p y ,p z ] T The positional errors corresponding to a and p are Δa = [Δa1, Δa2, ..., Δa], respectively. m ,…,Δa M ] T , Δp=[Δp x ,Δp y ,Δp z ] T The standard deviations of Δa and Δp are σ a σ p The moving solid body, in a global reference frame, contains directional, positional, angular velocity, and velocity information with errors, respectively, q = [α, β, γ]. T , t = [t x ,t y ,t z ] T w = [w x ,w y ,w z ] T , Where N≥5, M≥6, c n This represents the position coordinates of the nth sensor in the local reference frame. a m Let a represent the position coordinates of the m-th anchor node in the global reference frame.m =[a mx ,a my ,a mz ],Δa m Indicates a m The corresponding positional error, Δa m =[Δa mx ,Δa my ,Δa mz ], α, β, γ represent the yaw angle, roll angle, and pitch angle of the solid body in the global reference frame. T Indicates the transpose operation;

[0008] (2) Construct the measurement vector:

[0009] The position coordinates c of each sensor in the local reference frame n The position coordinates of each anchor node in the global reference frame, a m The moving solid contains error-related direction information q, position information t, angular velocity information w, and velocity information. Calculate the distance measurement r between each sensor and each anchor node. mn and Doppler measurements The distance measurement vector r = [r] is obtained. 11 ,r 21 ,…,r mn ,…,r MN ] T and Doppler measurement vector Simultaneously, by using the position information of each anchor node in the global coordinate system a m Given the location information p of the calibration source, calculate the distance measurement d between each anchor node and the calibration source. m This yields the distance measurement vector d = [d1, d2, ..., d]. m ,…,d M ] T And through r, d, along with the location information p of the calibration source and the position coordinates a of the M anchor nodes in step (1), construct the measurement vector.

[0010] (3) Construct the estimation vector and obtain the maximum likelihood solution of the estimation vector:

[0011] (3a) Using the orientation estimate, position estimate, angular velocity estimate, and velocity estimate q of the solid body in the global reference frame o t o w o , And the estimated value a of the coordinates a of the M anchor nodes. o And the estimate of the source location coordinates p oConstruct the estimation vector Simultaneously, the distance measurement vector r between M anchor nodes and N sensors, and the Doppler measurement vector... The error covariance matrix R v , The covariance matrix R of the position errors Δa of the M anchor nodes a The covariance matrix R of the calibration source position error Δp p The distance measurement error covariance matrix R between M anchor nodes and the calibration source d Construct the measurement error matrix Where Bdiag[] represents a block diagonal matrix;

[0012] (3b) By measuring vector ζ and estimating vector ψ o Given the measurement error matrix R, construct a least squares model ψ:

[0013] ψ = arg ψ min(ζ-ζ(ψ o )) T R -1 (ζ-ζ(ψ o ))

[0014] Where, ζ(ψ) o ) indicates that it contains the estimated vector ψ o The measurement vector, R -1 arg represents the inverse of the measurement error matrix R. ψ min(*) represents the value of ψ when (*) reaches its minimum value;

[0015] (3c) Based on the Gauss-Newton iteration rule, the measurement vector ζ(ψ) in the least squares model ψ o ) Perform k max The next update iteratively optimizes ψ to obtain the estimated vector ψ. o Maximum likelihood solution

[0016]

[0017] Where, k max ≥100;

[0018] (4) Obtain the positioning results of the moving solid body:

[0019] Through maximum likelihood solution Calculate the error-free orientation information of a moving solid body in a global reference frame. Location information angular velocity information Speed ​​information

[0020]

[0021]

[0022]

[0023]

[0024] Compared with the prior art, the present invention has the following advantages:

[0025] This invention introduces a calibration source into the positioning scenario and uses the distance measurement information between the calibration source and the anchor node when constructing the measurement vector. By updating the measurement vector in each iteration, the position error of the anchor node is continuously eliminated, effectively suppressing the deterioration of positioning accuracy caused by the position error of the anchor node, and thus improving the positioning accuracy of moving solid bodies. Attached Figure Description

[0026] Figure 1 This is a flowchart illustrating the implementation of the present invention.

[0027] Figure 2 This is a comparison chart showing the estimation errors of the direction information of a moving solid body between existing technologies and the present invention.

[0028] Figure 3 This is a comparison chart showing the estimation errors of the position information of a moving solid body between existing technologies and the present invention.

[0029] Figure 4 This is a comparison chart showing the estimation errors of the angular velocity information of a moving solid body between existing technologies and the present invention.

[0030] Figure 5 This is a comparison chart showing the estimation errors of velocity information of moving solid bodies between existing technologies and the present invention. Detailed Implementation

[0031] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0032] Reference Figure 1 The present invention includes the following steps:

[0033] Step 1) Construct the localization scene of the moving solid object and initialize the parameters:

[0034] Construct a 3D positioning scene for the moving solid body, which includes N sensors, M stationary anchor nodes distributed around it, and a calibration source; initialize the position coordinates of the N sensors in a local reference frame Oq1q2q3 with the geometric center of the moving solid body as the origin as c = [c1, c2, ..., c]. n,…,c N ] T The position coordinates of the M anchor nodes and calibration source in the global reference system Oxyz are respectively a = [a1, a2, ..., a...]. m ,…,a M ] T p = [p x ,p y ,p z ] T The positional errors corresponding to a and p are Δa = [Δa1, Δa2, ..., Δa], respectively. m ,…,Δa M ] T , Δp=[Δp x ,Δp y ,Δp z ] T The standard deviations of Δa and Δp are σ a σ p The moving solid body, in a global reference frame, contains directional, positional, angular velocity, and velocity information with errors, respectively, q = [α, β, γ]. T , t = [t x ,t y ,t z ] T w = [w x ,w y ,w z ] T , Where N≥5, M≥6, c n This represents the position coordinates of the nth sensor in the local reference frame. a m Let a represent the position coordinates of the m-th anchor node in the global reference frame. m =[a mx ,a my ,a mz ],Δa m Indicates a m The corresponding positional error, Δa m =[Δa mx ,Δa my ,Δa mz ], α, β, γ represent the yaw angle, roll angle, and pitch angle of the solid body in the global reference frame. T This represents the transpose operation. In this example, N=5, M=6, and the position coordinates of all N sensors are:

[0035]

[0036] Each column in c represents the position coordinates of each sensor. The positions of the calibration source and anchor nodes are randomly generated, but the distance between any two anchor nodes and between the calibration source and each anchor node must not be less than 15 meters. The standard deviation σ of the position error Δa of all M anchor nodes is... a The standard deviation σ of the calibration source position error Δp p The value is 10 -2 ≤σ a ≤10 0.5 σ p =10 -0.5 The moving solid body contains position information t, angular velocity information w, and velocity information with errors in a global reference frame. Yaw angle α, roll angle β, and pitch angle γ are respectively t=[100,100,50]. T Meters, w = [0.1, 0.2, 0.3] T radians per second meters per second, α = 20 degrees, β = -25 degrees, γ = 10 degrees;

[0037] Step 2) Construct the measurement vector in the positioning scenario:

[0038] The position coordinates c of each sensor in the local reference frame n The position coordinates of each anchor node in the global reference frame, a m The moving solid contains error-related direction information q, position information t, angular velocity information w, and velocity information. Calculate the distance measurement r between each sensor and each anchor node. mn and Doppler measurements All r mn , Representing each column as a matrix, we obtain the distance measurement vector r = [r 11 ,r 21 ,…,r mn ,…,r MN ] T and Doppler measurement vector Simultaneously, by using the position information of each anchor node in the global coordinate system a m Given the location information p of the calibration source, calculate the distance measurement d between each anchor node and the calibration source. m , all of the d m Representing the distances as a matrix, we obtain the distance measurement vector d = [d1, d2, ..., dn]. m ,…,d M ] T And through r, d, along with the location information p of the calibration source and the position coordinates a of the M anchor nodes in step (1), construct the measurement vector. Where, rmn , and d m The calculation formulas are as follows:

[0039] r mn =||a m -Qc n -t||

[0040]

[0041]

[0042] d m =||pa m ||

[0043] Where Q is the angle matrix of the moving solid body in the global reference frame, including errors, |||| denotes the Euclidean norm, and [w] × Let w be the cross product matrix of the angular velocity information w of the solid body in the global reference frame;

[0044] Step 3) Construct the estimation vector and obtain the maximum likelihood solution of the estimation vector:

[0045] Step 3a) Using the orientation estimate, position estimate, angular velocity estimate, and velocity estimate q of the solid body in the global reference frame. o t o w o , And the estimated value a of the coordinates a of the M anchor nodes. o And the estimate of the source location coordinates p o Combine all the above estimators column-wise into a column vector to obtain the estimation vector. Simultaneously, the standard deviation σ of the error of the distance measurement vector r between M anchor nodes and N sensors is used. r Doppler measurement vector standard deviation of error And the standard deviation σ of the distance measurement vector d between the M anchor nodes and the calibration source d , construct r, The error covariance matrices of d are R v , R d The standard deviation σ of the position error Δa of M anchor nodes a The standard deviation σ of the calibration source position error Δp p Construct the covariance matrices of Δa and Δp as R, respectively. a R p , will R v , R a R dand R p Place them sequentially on the main diagonal of the zero matrix to obtain Where Bdiag[] represents a block diagonal matrix, R v , R a R d and R p The specific expression is

[0046]

[0047]

[0048]

[0049]

[0050]

[0051] Among them, I MN×MN Let I be an identity matrix of dimension MN×MN. 3M×3M Let I be a 3M×3M identity matrix. M×M I represents an M×M identity matrix. 3×3 This represents a 3×3 identity matrix. For the Kronecker product; in this example, σ r =10 -2 , σ r =10 -3 , σ d =10 -2 ;

[0052] Step 3b) By measuring vector ζ and estimating vector ψ o Given the measurement error matrix R, construct a least squares model ψ:

[0053] ψ = arg ψ min(ζ-ζ(ψ o )) T R -1 (ζ-ζ(ψ o ))

[0054] Where, ζ(ψ) o ) indicates that it contains the estimated vector ψ o The measurement vector, R -1 arg represents the inverse of the measurement error matrix R. ψ min(*) represents the value of ψ when (*) reaches its minimum value;

[0055] Step 3c) Based on the Gauss-Newton iteration rule, set the initial value of the iteration to ψ. {0}For the measurement vector ζ(ψ) in the least squares model ψ o ) Perform k max The next update gradually eliminates the distance measurement vector r and Doppler measurement vector between all N sensors and all M anchor nodes. The errors of ψ, the position errors of the M anchor nodes Δa, and the position error of the calibration source Δp are used to achieve iterative optimization of ψ, ultimately obtaining the estimated vector ψ. o Maximum likelihood solution

[0056]

[0057] Where, k max ≥100, ζ(ψ) o The update formula for ) is:

[0058]

[0059] ψ {0} Let ζ(ψ) be the initial value for the iteration. {0} ) contains the initial value ψ of the iteration {0} Measurement vector, G {0} The gradient matrix is:

[0060]

[0061] Represents the distance measurement vector r and the Doppler measurement vector between all M anchor nodes and all N sensors. All estimates for solids Estimation of the positions a of M anchor nodes a o Find the partial derivative. The distance measurement vector d between all M anchor nodes and the calibration source is represented by the estimate of the positions a of the M anchor nodes. o The estimate of the calibration source location p o Find the partial derivative, where O denotes the zero matrix; in this example, k max =100;

[0062] Step 4) Obtain the positioning results of the moving solid object:

[0063] Through maximum likelihood solution Calculate the error-free orientation information of a moving solid body in a global reference frame. Location information angular velocity information Speed ​​information

[0064]

[0065]

[0066]

[0067]

[0068] The technical effects of the present invention will be further explained below with reference to simulation results:

[0069] 1. Simulation conditions and content:

[0070] The simulation experiment software platform is: Windows 10 operating system, Matlab version 2018.

[0071] Simulations were performed on the estimation errors of the orientation, position, angular velocity, and velocity information of this invention and an existing error-eliminating method for locating moving solid bodies. The results are as follows: Figure 2 , Figure 3 , Figure 4 , Figure 5 As shown.

[0072] 2. Analysis of experimental results:

[0073] Reference Figure 2 , Figure 3 , Figure 4 , Figure 5 The horizontal axis represents the noise intensity at the anchor node location, and the vertical axis represents the estimation errors of the moving solid body's direction, position, angular velocity, and velocity information, respectively. Figure 2-5 As can be seen, with the increase of noise intensity at the anchor node position, the estimation errors of the direction, position, angular velocity, and velocity information of the moving solid body obtained by the present invention and the prior art all increase. However, the estimation errors of the direction, position, angular velocity, and velocity information of the moving solid body obtained by the present invention are significantly smaller than those obtained by the prior art. When the noise intensity at the anchor node position is 10dB, the estimation errors of the direction, position, angular velocity, and velocity information of the moving solid body obtained by the prior art are 1.247 radians, 6.726 meters, 0.274 radians per second, and 0.36 meters per second, respectively, while the corresponding estimation errors of the present invention are only 0.057 radians, 5.073 meters, 0.013 radians per second, and 0.06 meters per second.

Claims

1. A method of motion solid body positioning based on a calibration source, characterized by, Comprising the steps of: (1) constructing a positioning scenario of the moving solid body and initializing parameters: The build includes the installation of The sensors are moving solid objects, and the sensors distributed around the moving solid objects are stationary. A three-dimensional positioning scene of a moving solid body with one anchor node and one calibration source; initialization of the... A sensor in a local reference frame with the geometric center of the moving solid as the origin. The position coordinates below are The Anchor nodes and calibration sources are located in the global reference system. The position coordinates below are respectively , , , The corresponding position errors are respectively , , , The standard deviations are respectively , The moving solid body contains directional, positional, angular velocity, and velocity information with errors in a global reference frame, respectively. , , , ;in, , , Indicates the first The position coordinates of a sensor in a local reference frame , Indicates the first The position coordinates of each anchor node in the global reference frame. , express The corresponding positional error, , , , This indicates the yaw angle, roll angle, and pitch angle of the solid body in a global reference frame. Indicates the transpose operation; (2) constructing a measurement vector: Using the position coordinates of each sensor in the local reference frame The position coordinates of each anchor node in the global reference frame And the directional information of moving solids containing errors Location information Angular velocity information and speed information Calculate the distance measurement between each sensor and each anchor node. and Doppler measurements The distance measurement vector is obtained. and Doppler measurement vector Simultaneously, by using the location information of each anchor node in the global coordinate system... and location information of calibration source Calculate the distance measurement between each anchor node and the calibration source. The distance measurement vector is obtained. and through , , and the location information of the calibration source in step (1) and The location coordinates of each anchor node Construct measurement vectors ; (3) constructing an estimation vector and obtaining a maximum likelihood solution of the estimation vector: (3a) Using the orientation, position, angular velocity, and velocity estimates of a solid body in a global reference frame , , , , as well as Anchor node location coordinates estimator and calibration source location coordinates estimator Construct the estimation vector , ; at the same time through Anchor nodes and Distance measurement vector between sensors Doppler measurement vector Error covariance matrix , , Anchor node position error covariance matrix Calibration source position error covariance matrix , The distance measurement error covariance matrix between each anchor node and the calibration source Construct the measurement error matrix ,in, Represents a block diagonal matrix; (3b) Constructing a least squares model by measuring vectors , estimating vectors and a measurement error matrix :​ ； wherein denotes a measurement vector comprising an estimated vector , denotes an inverse matrix of a measurement error matrix , denotes the value of when the minimum is taken. (3c) Based on the Gauss-Newton iteration rule, the least squares model is... Measurement vector in conduct This update will enable... Through iterative optimization, the estimated vector is obtained. Maximum likelihood solution : ； wherein ; (4) obtaining a positioning result of the moving solid body: by maximum likelihood solution calculating error-free directional information of a moving solid body in a global reference system , position information , angular velocity information , velocity information : ； ； ； 。 2. The calibration source based motion solid body positioning method of claim 1, wherein, calculating the distance measurement between each sensor and each anchor node and Doppler measurements and the distance measurement between each anchor node and the calibration source respectively ； ； ； ； wherein, is the angular matrix of the moving solid body containing errors in the global frame, denotes the Euclidean norm, is the angular velocity information of the solid body in the global frame is the cross product matrix.

3. The calibration source based motion solid body positioning method of claim 1, wherein, The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) , The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) , The distance measurement vector between the anchor node and the sensor in step (3a) The distance measurement vector between the anchor node and the sensor in step (3a) ； ； ； ； ； wherein , is the error standard deviation of the distance measurement vector between the anchor nodes and the sensors, the error standard deviation of the Doppler measurement vector, is the error standard deviation of the distance measurement vector between the anchor nodes and the calibration source, is the identity matrix of dimension , is the identity matrix of dimension , denotes the identity matrix of dimension , denotes the identity matrix of dimension , is the Kronecker product.​ 4. The calibration source based motion solid body positioning method of claim 1, wherein, The update of the measurement vector in step (3c) is given by the update formula ； ； wherein is the iteration initial value, is the measurement vector containing the iteration initial value , is the gradient matrix, , , , denotes the distance measurement vector between all anchor nodes and all sensors, the Doppler measurement vector all estimates of the solid body, , the estimate of the anchor node positions , the partial derivative of, , denotes the distance measurement vector between all anchor nodes and the calibration source, the estimate of the anchor node positions , the estimate of the calibration source position , the partial derivative of, denotes the zero matrix.

Images