CN115792796B

CN115792796B - Co-location method, device and terminal based on relative observation equivalent model

Info

Publication number: CN115792796B
Application number: CN202310104101.2A
Authority: CN
Inventors: 孙涛; 莫继学; 崔金强
Original assignee: Peng Cheng Laboratory
Current assignee: Peng Cheng Laboratory
Priority date: 2023-02-13
Filing date: 2023-02-13
Publication date: 2023-06-06
Anticipated expiration: 2043-02-13
Also published as: CN115792796A

Abstract

The invention discloses a co-location method, a device and a terminal based on a relative observation equivalent model, which comprise the following steps: predicting the position information of the current platform through a first sensor, observing surrounding anchor points through a second sensor, and updating the predicted position information; detecting other unmanned systems within a preset communication range, and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm; based on an equivalent observation model of relative observation, the position information of the current platform is updated secondarily or in a consistency fusion mode by utilizing the relative observation of the current platform and other unmanned systems; and taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting, and outputting the position information of the current platform at the next moment. The invention strengthens the positioning of the unmanned system platform in a non-trace transformation and fusion mode, thereby improving the positioning precision and ensuring the consistency of the result.

Description

Co-location method, device and terminal based on relative observation equivalent model

Technical Field

The present invention relates to the field of co-location technologies, and in particular, to a co-location method, device, and terminal based on a relative observation equivalent model.

Background

In recent years, as performance of unmanned platforms and sensor technologies is improved, a co-location algorithm based on a multi-unmanned system network is unprecedented. Currently, there are two main implementations of co-location algorithms based on multi-unmanned system networks, namely a centralized architecture (Centralized Framework, CF) and a distributed architecture (Distributed Framework, DF). The centralized architecture is composed of an information Fusion Center (FC, usually served by a master unmanned system) and other unmanned systems with sensors, and the information Fusion Center and each unmanned system need to establish communication. While CF architecture provides optimal positioning effect given the sensor accuracy, it introduces a number of limitations to the application because the information fusion center needs to bear all the computational load while still establishing and maintaining direct communication with the various unmanned systems in the network. Then, the distributed co-location framework receives more attention, and the unmanned system can observe surrounding anchor points according to the carried sensors (such as distance and angle sensors) so as to solve the position information of the unmanned system. Due to the influence of environmental factors (such as the influence of electromagnetic environmental factors on the observed value), large errors can occur in self-observation, so that positioning failure or positioning accuracy is reduced. On the other hand, the positioning accuracy can be improved through the relative observation among different unmanned systems. The relative observations only occur between unmanned systems that are adjacent to each other and that can establish communications. The establishment of relative observation and mutual communication mechanisms enables the unmanned system to reduce problems caused by own sensor abnormality or enhance own positioning accuracy. Each platform in the unmanned system network is distributed at different positions, and each unmanned system only communicates with the adjacent unmanned system platform. This greatly reduces the requirements for communication (such as communication bandwidth). Finally, each unmanned system can independently bear some calculation tasks, and a fusion center is not needed to process all data in a centralized manner. The many advantages of the distributed architecture make distributed co-location based on multi-unmanned system networks a hotspot for research in recent years.

Since the relative observations occur between unmanned systems that can establish communications. In addition to the relative observations, neighboring unmanned systems need to send their own location information via a communication mechanism. Therefore, the mutual communication unmanned systems can obtain the absolute position coordinates of the other side, namely the unmanned systems can act as anchor points mutually, so that the positioning accuracy is improved, or the problems that the positioning accuracy is reduced due to the large error of the sensor are solved. But there is always Uncertainty (uncrtainty) of the location information of the unmanned system to itself. The positioning information is directly utilized as an anchor point which can be utilized by a counterpart, so that a larger error can be obtained. The problem is that the influence of uncertainty of the positioning coordinates is not considered, so that complete and accurate noise characteristic information cannot be obtained while the relative observation filtering is utilized. The result obtained at this time often cannot meet the Consistency requirement, i.e. the positioning result obtained underestimates its error, so that the covariance matrix is smaller than the actual one. Over time, large errors can occur in the results of the self-localization and the actual true values. As direct updating with relative observations has negative impact. There are a number of problems with the centralized state estimation framework.

Accordingly, there is a need in the art for improvement.

Disclosure of Invention

The invention aims to solve the technical problem that the prior art has low co-location precision due to the relative observation, and provides a co-location method, a device and a terminal based on a relative observation equivalent model.

The technical scheme adopted for solving the technical problems is as follows:

in a first aspect, the present invention provides a co-location method based on a relative observation equivalent model, including:

predicting the position information of the current platform in a motion state through a first sensor, observing surrounding anchor points through a second sensor, and updating the predicted position information;

detecting other unmanned systems within a preset communication range, and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm;

based on the equivalent observation model of the relative observation, the position information of the current platform is updated secondarily or is updated in a consistency fusion mode by utilizing the relative observation of the current platform and other unmanned systems;

and taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting according to the positioning information of the current moment, and outputting the position information of the current platform at the next moment.

In one implementation, the first sensor includes: an IMU sensor; the second sensor includes: a distance and angle sensor;

the predicting, by the first sensor, the position information of the current platform in the motion state, and observing surrounding anchor points by using the second sensor, and updating the predicted position information, including:

predicting the position state and the posture state of the current platform in a motion state through the IMU sensor;

measuring the distance and the relative pose of the current platform and surrounding anchor points by using the distance and angle sensor;

and updating the predicted position information by using an updating equation of an extended Kalman filter or an unscented filter according to the predicted position state, the predicted posture state and the measured distance.

In one implementation, the state equations for the position state and the attitude state are:

wherein ,

the system state to be estimated;

a nonlinear function of the state of the current platform;

is Gaussian white noise;

k is the discrete time.

In one implementation, the updated equation of the kalman filter is:

wherein ,

positioning information of the current moment;

The predicted value of the k-1 moment to the k moment; />

The state or positioning information updated by the anchor point is utilized for the moment k;

an observation value of the anchor point for the current platform;

is the observed predicted value;

respectively an observed covariance matrix and a cross covariance matrix;

r is the covariance matrix of the sensor white noise.

In one implementation manner, the detecting other unmanned systems within a preset communication range and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm includes:

detecting whether other unmanned systems exist in the preset communication range;

if other unmanned systems are detected, sending the position information estimated quantity of the current platform at the current moment to the other unmanned systems, and acquiring the position information estimated quantity of the other unmanned systems at the current moment;

and establishing an equivalent observation model of the relative observation according to the position information estimated quantity of the current time of exchange and the unscented transformation algorithm.

In one implementation, the establishing the equivalent observation model of the relative observation according to the exchanged position information estimator at the current time and the unscented transformation algorithm includes:

acquiring Sigma transformation points corresponding to the position information estimators of other unmanned systems through unscented transformation;

And calculating uncertainty according to all the transformed points and establishing an equivalent observation model of the relative observation.

In one implementation, the obtaining, through unscented transformation, sigma transformation points corresponding to the estimated position information of the other unmanned systems includes:

generating a set of sigma points characterizing a probability distribution by an unscented transformation:

wherein n is the dimension of the system state;

wherein ,

for the corresponding matrix->

Is the ith column of the square root matrix;

is a pending parameter;

(1:2) the first two positioning coordinates of one of the columns;

weights corresponding to each Sigma point

Expressed as:

/>

and (3) obtaining corresponding Sigma transformation points from a point set consisting of all Sigma points through an original relative observation model:

wherein ,

for Sigma Point->

Is a first dimension and a second dimension of the (c).

In one implementation, the calculating uncertainty and building the equivalent observation model of the relative observation from all transformed points includes:

from all transformed points, uncertainty is calculated:

is a mean square error matrix;

the original relative observation model is set as:

wherein

An estimated amount of positioning information;

is equivalent noise.

In one implementation manner, the performing, by using the relative observation of the current platform and other unmanned systems, the second updating of the location information of the current platform based on the equivalent observation model of the relative observation includes:

Let the equivalent noise be:

wherein ,

；

transforming the equivalent observation model of the relative observation:

determining a secondary updating equation according to the transformed equivalent observation model:

wherein ,

obtaining a result of secondary updating of the positioning information by using the relative observation;

is a predicted value for relative observation;

and />

A covariance matrix of the relative observation and a cross covariance matrix of the relative observation and positioning information; />

wherein

Is a nonlinear model of the relative observations.

In one implementation manner, the updating the position information of the current platform by using the relative observation of the current platform and other unmanned systems through a consistency fusion mode based on the equivalent observation model of the relative observation includes:

for predictionEstimation of the quantity

Updating, and representing the obtained updated positioning state as

；

Updating the obtained update state and the anchor point to obtain the positioning state

And (3) carrying out consistency fusion:

wherein ,

，/>

as a result after fusion, subscript F represents fusion; />

Representation of matrix->

Performing inversion; />

Representing parameters between 0 and 1.

In a second aspect, the present invention provides a co-locating device based on a relative observation equivalent model, comprising:

the primary updating module is used for predicting the position information of the current platform in a motion state through the first sensor, observing surrounding anchor points through the second sensor and updating the predicted position information;

The equivalent observation module is used for detecting other unmanned systems within a preset communication range and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm;

the secondary updating module is used for carrying out secondary updating on the position information of the current platform or updating the position information of the current platform in a consistency fusion mode by utilizing the relative observation of the current platform and other unmanned systems based on the equivalent observation model of the relative observation;

and the position prediction module is used for taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting according to the positioning information of the current moment and outputting the position information of the current platform at the next moment.

In a third aspect, the present invention provides a computer terminal comprising: a processor and a memory storing a co-location program based on a relative-observation-equivalent model, which when executed by the processor is operative to implement the co-location method based on a relative-observation-equivalent model as described in the first aspect.

In a fourth aspect, the present invention also provides a computer-readable storage medium storing a co-location program based on a relative-observation equivalent model, which when executed by a processor is configured to implement the operations of the co-location method based on a relative-observation equivalent model as described in the first aspect.

The technical scheme adopted by the invention has the following effects:

the invention utilizes the unscented transformation to obtain the relative observation equivalent model in the co-location of a plurality of unmanned systems, and carries out secondary update on the position information of the unmanned systems based on the equivalent model in a fusion mode.

Drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to the structures shown in these drawings without inventive effort for a person skilled in the art.

FIG. 1 is a flow chart of a co-location method based on a relative observed equivalent model in one implementation of the invention.

FIG. 2 is a 2-D schematic diagram of a multi-unmanned system platform co-location based on relative observations in one implementation of the invention.

FIG. 3 is a flow chart of a multi-unmanned system co-location algorithm based on relative observations in one implementation of the invention.

Fig. 4 is a functional schematic of a terminal in one implementation of the invention.

The achievement of the objects, functional features and advantages of the present invention will be further described with reference to the accompanying drawings, in conjunction with the embodiments.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more clear and clear, the present invention will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.

Exemplary method

In the existing co-location method of multiple unmanned systems for relative observation, uncertainty (Uncertinty) exists in the location information of the unmanned system. The positioning information is directly utilized as an anchor point which can be utilized by a counterpart, so that a larger error can be obtained. The problem is that the influence of uncertainty of the positioning coordinates is not considered, so that complete and accurate noise characteristic information cannot be obtained while the relative observation filtering is utilized. The result obtained at this time often cannot meet the Consistency requirement, i.e. the positioning result obtained underestimates its error, so that the covariance matrix is smaller than the actual one. Over time, large errors can occur in the results of the self-localization and the actual true values.

Aiming at the technical problems, the embodiment provides a collaborative positioning method based on a relative observation equivalent model, in the embodiment, the relative observation equivalent model in the multi-unmanned system collaborative positioning is obtained by utilizing the unscented transformation, and the position information of the unmanned system is updated secondarily by fusion based on the equivalent model.

In this embodiment, the co-location method based on the relative observation equivalent model is applied to a terminal, where the terminal includes but is not limited to: unmanned systems (e.g., unmanned mobile terminals), computer devices, intelligent mobile terminal devices, and the like.

As shown in fig. 1, an embodiment of the present invention provides a co-location method based on a relative observation equivalent model, including the following steps:

step S100, predicting the position information of the current platform in a motion state through a first sensor, observing surrounding anchor points through a second sensor, and updating the predicted position information.

In this embodiment, each moving unmanned platform predicts its own position information in the motion process through a first sensor (for example, inertial Measurement Unit, IMU sensor) carried by itself, and at the same time, observes surrounding Anchor points (Anchor) through a second sensor (for example, a distance and angle sensor), and updates its own position information through an extended kalman filter (Extended Kalman Filter, EKF). This process relies on the reliability of the anchor observation information. Abnormal data (Outlier) such as those encountered with large errors will produce large errors in positioning.

Specifically, in one implementation of the present embodiment, step S100 includes the steps of:

step S110, predicting the position state and the posture state of the current platform in a motion state through the IMU sensor;

step S120, measuring the distance and the relative pose of the current platform and surrounding anchor points by using the distance and angle sensor;

step S130, according to the predicted position state, the predicted attitude state and the measured distance, the predicted position information is updated by using an update equation of an extended Kalman filter or an unscented filter.

In this embodiment, each node in the unmanned system network is an unmanned platform on which a sensor is mounted, and has an independent operation processing unit and communication unit. The linking manner of all nodes in the unmanned system network can be represented by an undirected graph. Definition graph g= (V, E), V is a set of all nodes, E is a set of all edges, and two node unmanned system platforms linked by each edge can communicate. The number of nodes that each unmanned system platform can communicate with is referred to as the degree. The set of nodes s and neighboring nodes is represented as

。/>

Is a set->

The sum of the number of the intermediate nodes, namely the degree of the node s. As shown in fig. 2, a schematic diagram of a simple unmanned system network node link is given in fig. 2.

As shown in fig. 3, the present embodiment is described with reference to the relative observation-based multi-unmanned system co-location algorithm in fig. 3:

step one as shown in fig. 3:

first, each unmanned system platform runs an Extended Kalman Filter (EKF) independently. The EKF is divided into a prediction part and an update part. In the prediction part, the unmanned platform predicts the states of the unmanned platform, such as the position, the posture and the like according to the sensor IMU (including an accelerometer and a gyroscope) carried by the unmanned platform. The uncertainty of the state is given by the covariance matrix of the sensor error. The state equation of the system is described as (the system is described as a discrete system, with the subscript k indicating the time at this moment):

(1)

wherein ,

is the state of the system to be estimated (including unmanned system position, attitude, etc., as vectors, in bold). Taking the 5-dimensional unmanned system platform state as an example, i.e. the position in the x-y direction +.>

And corresponding speed->

And unmanned platform attitude angle->

Thus->

The amount of state estimation is expressed as

Where T represents the transpose of the vector. />

Is a nonlinear function of the state of the system. />

Is Gaussian white noise, usually expressed as +.>

(mean value of noise is 0, covariance is +.>

). This equation is discrete, with k representing the discrete time (i.e., 1,2,3, …). In the case of a continuous system, the time variable is denoted by t. The state equation is a differential equation, and the algorithm is still applicable. And will not be described in detail herein.

In the motion process of the unmanned system platform, other sensors, such as a distance sensor and an angle sensor, are used for measuring the distance and the relative posture between the unmanned system platform and surrounding anchor points. Where the anchor points are fixed and have points of determined location, the presence of the anchor points may serve as a reference to the unmanned platform's own positioning, such as the star points shown in fig. 2. Due to the errors of the distance and angle sensors, the observations given by the sensors alone do not allow a good positioning. The estimated value of unmanned platform prediction and the observation given by the current sensor need to be updated with the update equation of EKF. The update equation is shown below (taking unmanned platform a as an example, assume that the positioning information is now

, wherein />

Representing the predicted value of time k-1 versus time k at this time):

(2)

(3)

wherein ,

the state or positioning information updated by the anchor point is utilized for time k (subscript a indicates that this update comes from the anchor point observation). />

The observation value of the unmanned system platform A to the anchor point is directly derived from the used sensor.

Observed predictive value. />

Is the observed covariance matrix and the cross covariance matrix. R is the covariance matrix of the sensor white noise.

As shown in fig. 1, in an implementation manner of the embodiment of the present invention, the co-location method based on the relative observation equivalent model further includes the following steps:

and step S200, detecting other unmanned systems within a preset communication range, and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm.

In this embodiment, by detecting other unmanned systems within a preset communication range, an equivalent observation model of relative observation can be established; when two or more unmanned system platforms are operating, their relative observations can be obtained with the sensors within communication range of each other and within measurement range of the sensors, while the communication mechanism is used to exchange positional information with each other at that moment (where this positional information is the result of being updated based on anchor point information at the moment, uncertainty is given by EKF).

In this embodiment, the UT method is used to build an equivalent observation model for relative observation. The equivalent model measures the error caused by uncertainty in the position of the unmanned platform with which it communicates and gives it in the form of noise. This process is equivalent to using the unmanned system that generated the relative observations as an additional anchor point, but this anchor point can only become a "false anchor point" due to the uncertainty that the unmanned system location carries. The effect of its uncertainty needs to be taken into account in the new relative observation model. In addition, if a plurality of unmanned platforms are encountered to simultaneously establish relative observation and communication, each unmanned platform can send self-positioning information (including information such as position information, uncertainty and the like) to all unmanned platforms with which communication is established. The unmanned system receiving the information can establish relative observation equivalent models for other unmanned system platforms.

Specifically, in one implementation of the present embodiment, step S200 includes the steps of:

step S210, detecting whether other unmanned systems exist in the preset communication range;

step S220, if other unmanned systems are detected, sending the estimated position information of the current platform at the current moment to the other unmanned systems, and acquiring the estimated position information of the other unmanned systems at the current moment;

And step S230, establishing an equivalent observation model of the relative observation according to the exchanged position information estimated quantity at the current moment and the unscented transformation algorithm.

Step two as shown in fig. 3:

each unmanned system platform locates itself using the anchor points. If the sensor of the system observes other unmanned system platforms and is in the communication range at the moment, the communication can be initiated to estimate the position information at the moment

and />

(for ease of presentation, k at the time indicated in the subscript has been removed) for exchange. For example, unmanned system platform A observes unmanned system B within communication range, with a relative observation of +.>

(subscript AB indicates that unmanned platform a's sensor observed unmanned platform B, and the relative observation was from a). Unmanned platform A obtains the state value of B at the current time +.>

A utilizes UT transformation to calculate a relative observation equivalent model. The original relative observation model (nonlinear model) is:

(4)

in this case, the distance relative observation is taken as an example, and other relative observations (such as angle observations) are also applicable. Which is a kind ofIn (a)

、/>

Positional information at this point (here, two-dimensional planar problem, the algorithm is equally applicable if three-dimensional problem is considered) for unmanned platform a and unmanned platform B, respectively. Since the position information of the unmanned platform is an estimated quantity (i.e. an estimated quantity of positioning, and thus contains errors, the errors are represented by a mean square error matrix +. >

Expression) of->

For observing noise->

R is the mean square error of its noise.

Since the original relative measurement model cannot be directly utilized, the traditional method directly estimates the quantity

Substitute +.>

. However, since all estimates still have errors, model I does not express the relative observations well. It is therefore necessary to process it, taking into account the uncertainties contained in the estimators, making the model more representative of the real situation and of the reaction uncertainties. UT transformation gives a measure of uncertainty of random variables under nonlinear transformation, which is as follows:

in one implementation of the present embodiment, step S230 includes the steps of:

step S231, the Sigma transformation points corresponding to the position information estimators of other unmanned systems are obtained through unscented transformation.

In this embodiment, the UT transform needs to be given

And the uncertainty carried by it +.>

In this case, the transformation of sigma points representing the probability distribution mainly comprises:

in step S231a, a set of sigma points characterizing a probability distribution is generated by an unscented transformation.

Step S231b, the point set composed of all Sigma points is used for obtaining corresponding Sigma transformation points through the original relative observation model.

In this embodiment, a set of sigma points characterizing a probability distribution is first generated by an unscented transformation

Where n is the dimension of the system state. The unscented transform UT is used as the basis of the unscented filter, and has the main forms: />

(5)

wherein ,

for the corresponding matrix->

Is the i-th column of the square root matrix. The square root matrix of a matrix can be obtained by Cholesky decomposition (e.g.)>

Matrix M is called the square root of matrix P)>

Is a pending parameter. (1:2) represents the first two elements of one column, since the x-y plane positioning coordinates are discussed herein, which correspond to the first two elements. Weight for each Sigma Point->

Expressed as:

(6)

the point set consisting of 2n+1 Sigma points obtains corresponding Sigma transformation points through the original relative observation model

：

(7)

wherein ,

for Sigma Point->

Because the first and second dimensions of the state represent the position of the x-y plane. Note that here the transformed Sigma point +.>

And original Sigma Point>

Are of different dimensions.

In one implementation of this embodiment, step S230 further includes the following steps:

and step S232, calculating uncertainty according to all the transformed points and establishing an equivalent observation model of the relative observation.

In the present embodiment, after transformation, uncertainty is calculated from the transformed points

And establishing an equivalent model.

In one implementation of this embodiment, step S232 further includes the steps of:

step S232a, calculating uncertainty from all transformed points.

Step S232b, the original relative observation model is improved.

In this embodiment, the uncertainty is calculated by:

(8)

(9)

the uncertainty caused by the error in the positioning state can be thus taken as a mean square error matrix

Is a noise expression of (a). Because the noise of the sensor itself and this equivalent noise are independent of each other, they can be superimposed on each other. The original relative observation model is changed into: />

(10)

wherein

Is an estimate of the positioning information. />

Is equivalent noise, and its statistical probability is described as

. The analysis of the angle measurement model is also omitted here in a similar way, the following analysis taking distance observations only as an example.

After the equivalent noise is introduced, the new relative observation model can more accurately represent the statistical characteristics of the relative observation.

and step S300, based on the equivalent observation model of the relative observation, the position information of the current platform is updated secondarily or in a consistency fusion mode by utilizing the relative observation of the current platform and other unmanned systems.

In this embodiment, after the unmanned system obtains the corresponding information and calculates the equivalent model, the relative observation of the own platform to other unmanned systems is used to update the own position secondarily. If there is a relative observation with multiple unmanned systems, then the status updates may be performed sequentially or together. The updating mode can adopt an EKF technology, and can also be updated by using a consistency fusion mode. The consistency fusion mode is to fuse the positioning update by using the anchor point with the positioning update obtained by using the relative observation. Due to the common a priori estimate, there may be an unknown correlation between the two. The consistency fusion mode improves consistency based on relative observation positioning.

Specifically, in one implementation of the present embodiment, step S300 includes the steps of:

step S311, setting the equivalent noise as

。

Step S312, transforming the equivalent observation model of the relative observation.

Step S313, determining a secondary updating equation according to the transformed equivalent observation model.

Step three as shown in fig. 3:

the unmanned system platform passes through the obtained relative observation and the relative observation equivalent model obtained in the second step in fig. 3. And carrying out secondary updating on the self position information. The secondary update mode has the following two update modes:

The first updating mode is that for the positioning state updated based on the anchor point

And updating.

The equivalent noise at this point can be regarded as

, wherein

Because of->

Independent of each other and can therefore be added directly. The equivalent model type is as follows:

(11)

the updated equation using relative observations can be expressed as:

(12)

(13)

wherein ,

and obtaining a result of the second updating of the positioning information by using the relative observation. />

Is a predicted value for relative observation. />

and />

Covariance matrix of relative observation and cross covariance matrix of relative observation and positioning information respectively. />

The equivalent model can be obtained in a linearization mode, and also can be obtained in a UT mode, and the solving process is as follows:

(14)

(15)

(16)

wherein

The non-linear model of the relative observation, here the distance observation, is equally applicable to other observations (e.g. angular observations). Sigma Point>

Based on the estimated value updated with anchor point +.>

Produced, +.>

Replaced by

And (3) obtaining the product.

Specifically, in one implementation of the present embodiment, step S300 further includes the following steps:

step S321, estimating the predicted amount

Updating, and representing the obtained updated positioning state as +.>

。

Step S322, updating the obtained update status and the anchor point to obtain a positioning status

And carrying out consistency fusion.

In the present embodiment, the second updating mode is to predict the positioning state

And updating.

The updating method is to estimate the prediction

The updating is carried out, the updating process is similar to the first updating, the +.in (12-13)>

Replaced by->

And (3) obtaining the product. The resulting updated positioning state is denoted +.>

. The update status and the positioning status obtained by anchor update in step one of FIG. 3 +.>

And carrying out consistency fusion. The reason for the consistency fusion here is that there is a possible correlation between the positioning state obtained by the anchor point update and the positioning state obtained by the relative observation. To deal with this unknown correlation, a consistency fusion is required, and the fusion method may employ an inverse covariance intersection algorithm (Inverse Covariance Intersection, ICI).

The fusion equation is:

（17）/>

（18）

wherein ,

，/>

as a result after fusion, subscript F represents fusion; />

Representation of matrix->

Performing inversion; />

Representing parameters between 0 and 1.

In this embodiment, anchor point observation and relative measurement observation are fused by two different updating modes. The first way is to update the two observations sequentially. The resulting positioning state

There will be a better mean square error because the second update reduces the mean square error of the first update again. But ignores the unknown correlation that may exist for both observations. The resulting covariance matrix may be smaller than the true mean square error matrix, causing consistency problems.

The second method is to perform consistency fusion on the update results obtained by two kinds of observation respectively so as to obtain the positioning state

Has better consistency. However, in the second update method, matrix inversion is required, which results in an increase in the calculation amount. In practice, therefore, the selection should be made based on the computing resources.

and step S400, taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting according to the positioning information of the current moment, and outputting the position information of the current platform at the next moment.

In this embodiment, after each unmanned system updates its own location, the location information is used as the location information at the current time. And from there a prediction of the next step's position is made for the starting point, thereby returning to step 1 (as shown in fig. 3) to form a closed loop.

In this embodiment, the UT is first utilized to improve the relative observation model, and the positional uncertainty of the unmanned platform is measured in the relative observation model. Thereby enhancing reliability in utilizing relative observations. And secondly, when the positioning information of the unmanned platform is updated again by using the relative observation and the equivalent model, the positioning state can be updated in a fusion mode by using a consistency fusion technology, so that the possible inconsistency (incondensent) generated while the positioning precision is improved by using the relative observation is realized. And the whole multi-unmanned platform positioning process is realized in a completely distributed mode, each unmanned platform is only adjacent to the surrounding unmanned platforms which can generate communication to update relative observation, a specific fusion center is not needed in the whole system, and the overall robustness is improved.

The prior art has directly utilized the observed positioning information of the unmanned platform when considering the relative observations, ignoring the uncertainty it carries. Thus causing the relative observation model to not accurately characterize the actual situation and underestimating the uncertainty in the subsequent state update. The algorithm of the invention considers the uncertainty of the observed unmanned platform positioning state in the multi-machine relative observation. And the uncertainty of the positioning information is exchanged together in the communication. And the equivalent model of relative observation is realized by utilizing UT change, and uncertainty of positioning information is introduced into the relative observation model in an equivalent Gaussian white noise mode. The new model can more accurately represent the actual situation observed between unmanned platforms.

In this embodiment, two ways of updating the positioning state of the unmanned system platform are provided. The first updating mode is to update the unmanned platform based on anchor points by using relative observation

Performing a second update, the final result being denoted +.>

. The relative observed information is introduced into the positioning information estimation quantity through the UKF through the secondary updating, so that the accuracy is improved. The second updating mode is to predict the positioning information of the unmanned platform by using the relative observation +.>

Updating to obtain->

. Based on this result and the update status of the unmanned platform based on anchor points->

Consistency fusion (e.g., inverse covariance intersection method ICI) is performed. The estimated amount of final positioning information is denoted +.>

. The first update mode is simpler and more efficient, and consumes less computing resources. And secondly, due to the utilization of consistency fusion, more computing resources are consumed, but the problem of correlation between relative observation and anchor point observation can be solved, so that the consistency of results is ensured.

In other implementations of the present embodiment, the extended kalman filter may be changed to other nonlinear filters, such as a volume kalman filter (Cubature Kalman Filter, CKF), and the like, and this improvement changes the prediction mode of the single-node agent, and this improvement may be regarded as a modification of the present invention; alternatively, the parameters for consistency fusion using the inverse covariance cross fusion algorithm in the second update mode are replaced

And (5) setting. Such as the covariance matrix of the fusion result obtained using the parameters having minimal matrix traces, etc., this improvement can be considered as a denaturation scheme of the present invention.

The following technical effects are achieved through the technical scheme:

the embodiment utilizes unscented transformation to obtain a relative observation equivalent model in the co-location of multiple unmanned systems, and carries out secondary update on the position information of the unmanned systems based on the equivalent model in a fusion mode.

Exemplary apparatus

Based on the above embodiment, there is provided a co-locating device based on a relative observation equivalent model, including:

Based on the above embodiment, the present invention also provides a terminal, and a functional block diagram thereof may be shown in fig. 4.

The terminal comprises: the system comprises a processor, a memory, an interface, a display screen and a communication module which are connected through a system bus; wherein the processor of the terminal is configured to provide computing and control capabilities; the memory of the terminal comprises a storage medium and an internal memory; the storage medium stores an operating system and a computer program; the internal memory provides an environment for the operation of the operating system and computer programs in the storage medium; the interface is used for connecting external equipment such as mobile terminals, computers and other equipment; the display screen is used for displaying corresponding information; the communication module is used for communicating with a cloud server or a mobile terminal.

The computer program is executed by a processor to perform the operations of a co-location method based on a relative observed equivalent model.

It will be appreciated by those skilled in the art that the functional block diagram shown in fig. 4 is merely a block diagram of some of the structures associated with the present inventive arrangements and is not limiting of the terminal to which the present inventive arrangements may be applied, and that a particular terminal may include more or less components than those shown, or may combine some of the components, or have a different arrangement of components.

In one embodiment, a terminal is provided, including: the system comprises a processor and a memory, wherein the memory stores a co-location program based on a relative observation equivalent model, and the co-location program based on the relative observation equivalent model is used for realizing the operation of the co-location method based on the relative observation equivalent model when being executed by the processor.

In one embodiment, a storage medium is provided, wherein the storage medium stores a co-location program based on a relative-observation equivalent model, which when executed by a processor is operative to implement the co-location method based on a relative-observation equivalent model as described above.

Those skilled in the art will appreciate that implementing all or part of the above-described methods may be accomplished by way of a computer program comprising instructions for the relevant hardware, the computer program being stored on a non-volatile storage medium, the computer program when executed comprising the steps of the embodiments of the methods described above. Any reference to memory, database, or other medium used in embodiments provided herein may include non-volatile and/or volatile memory.

In summary, the invention provides a co-location method, a device and a terminal based on a relative observation equivalent model, wherein the method comprises the following steps: predicting the position information of the current platform in a motion state through a first sensor, observing surrounding anchor points through a second sensor, and updating the predicted position information; detecting other unmanned systems within a preset communication range, and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm; based on an equivalent observation model of relative observation, the relative observation of the current platform and other unmanned systems is utilized to update the position information of the current platform for the second time; and taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting according to the positioning information of the current moment, and outputting the position information of the current platform at the next moment. The invention strengthens the positioning of the unmanned system platform in a non-trace transformation and fusion mode, thereby improving the positioning precision and ensuring the consistency of the result.

It is to be understood that the invention is not limited in its application to the examples described above, but is capable of modification and variation in light of the above teachings by those skilled in the art, and that all such modifications and variations are intended to be included within the scope of the appended claims.

Claims

1. The co-location method based on the relative observation equivalent model is characterized by comprising the following steps:

taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting according to the positioning information of the current moment, and outputting the position information of the current platform at the next moment;

Detecting other unmanned systems in a preset communication range, and establishing an equivalent observation model of relative observation according to an unscented transformation algorithm, wherein the method comprises the following steps:

establishing an equivalent observation model of the relative observation according to the acquired position information estimated quantity at the current moment and the unscented transformation algorithm;

the establishing the equivalent observation model of the relative observation according to the acquired position information estimated quantity at the current moment and the unscented transformation algorithm comprises the following steps:

calculating uncertainty according to all the transformed points and establishing an equivalent observation model of the relative observation;

the method for obtaining Sigma transformation points corresponding to the position information estimators of other unmanned systems through unscented transformation comprises the following steps:

Wherein n is the dimension of the system state;

wherein ,

for the corresponding matrix->

Is the ith column of the square root matrix;

is a pending parameter;

(1:2) the first two positioning coordinates of the current column;

weights corresponding to each Sigma point

Expressed as: />

The corresponding Sigma transformation points are obtained through the point set consisting of all Sigma points by the relative observation model:

wherein ,

for Sigma Point->

Is a first dimension and a second dimension of (2);

calculating uncertainty according to all transformed points and establishing an equivalent observation model of the relative observation, wherein the equivalent observation model comprises the following steps:

from all transformed points, uncertainty is calculated:

is a mean square error matrix;

setting the relative observation model as:

wherein

An estimated amount of positioning information;

is equivalent noise;

the performing, by using the relative observation between the current platform and the other unmanned systems, the second updating of the position information of the current platform based on the equivalent observation model of the relative observation includes:

let the equivalent noise be:

wherein ,

；

transforming the equivalent observation model of the relative observation:

/>

wherein ,

Is a predicted value for relative observation;

and />

A covariance matrix of the relative observation and a cross covariance matrix of the relative observation and positioning information;

wherein

A nonlinear model of the relative observations;

the updating the position information of the current platform by using the relative observation of the current platform and other unmanned systems and a consistency fusion mode based on the equivalent observation model of the relative observation comprises the following steps:

for predictive estimators

Updating, and representing the obtained updated positioning state as

；

And (3) carrying out consistency fusion:

wherein ,

，/>

as a result after fusion, subscript F represents fusion; />

Representation of matrix->

Performing inversion; />

Representing parameters between 0 and 1.

2. The co-location method based on a relative observation equivalent model according to claim 1, wherein the first sensor comprises: an IMU sensor; the second sensor includes: a distance and angle sensor;

3. The co-location method based on a relative observation equivalent model according to claim 2, wherein the state equations of the position state and the posture state are:

wherein ,

the system state to be estimated;

a nonlinear function of the state of the current platform;

is Gaussian white noise;

k is the discrete time.

4. The co-location method based on a relative observation equivalent model according to claim 2, wherein the update equation of the extended kalman filter is:

wherein ,

positioning information of the current moment;

the predicted value of the k-1 moment to the k moment;

an observation value of the anchor point for the current platform;

Is the observed predicted value;

、/>

respectively an observed covariance matrix and a cross covariance matrix;

r is the covariance matrix of the sensor white noise.

5. A co-location device based on a relative observation equivalent model, comprising:

the position prediction module is used for taking the position information after the secondary updating or consistency fusion as the positioning information of the current moment, predicting according to the positioning information of the current moment and outputting the position information of the current platform at the next moment;

Wherein n is the dimension of the system state;

wherein ,

for the corresponding matrix->

Is the ith column of the square root matrix;

is a pending parameter;

(1:2) the first two positioning coordinates of the current column;

weights corresponding to each Sigma point

Expressed as:

wherein ,

for Sigma Point->

Is a first dimension and a second dimension of (2);

from all transformed points, uncertainty is calculated:

is a mean square error matrix;

setting the relative observation model as:

wherein

An estimated amount of positioning information;

is equivalent noise;

let the equivalent noise be:

wherein ,

；

transforming the equivalent observation model of the relative observation:

wherein ,

Is a predicted value for relative observation;

and />

/>

wherein

A nonlinear model of the relative observations;

for predictive estimators

Updating, and representing the obtained updated positioning state as

；

Performing consistency fusionAnd (3) combining:

wherein ,

，/>

as a result after fusion, subscript F represents fusion; />

Representation of matrix->

Performing inversion; />

Representing parameters between 0 and 1.

6. A computer terminal, comprising: a processor and a memory storing a co-location program based on a relative-observation equivalent model, which when executed by the processor is operative to implement the co-location method based on a relative-observation equivalent model as claimed in any one of claims 1 to 4.

7. A computer readable storage medium, characterized in that the medium stores a co-location program based on a relative-observation equivalent model, which co-location program, when executed by a processor, is adapted to carry out the operations of the co-location method based on a relative-observation equivalent model as claimed in any one of claims 1-4.