阅读说明：本技术 协同定位方法及定位系统 (Cooperative positioning method and positioning system ) 是由 李志天 张海峰 邹旭东 郑帅康 于 2021-08-17 设计创作，主要内容包括：本发明涉及一种协同定位方法及定位系统,所述定位方法包括：依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵；依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵；依据所述状态转移矩阵和所述测量矩阵进行融合定位,得到所述第一无人驾驶车相对于所述第二无人驾驶车的估计坐标。所述定位系统包括：第一无人驾驶车；第二无人驾驶车；所述第一无人驾驶车和所述第二无人驾驶车上分别安装有：惯性导航单元以及超宽带无线通信单元。(The invention relates to a cooperative positioning method and a positioning system, wherein the positioning method comprises the following steps: establishing a state transition matrix according to the action information of the first unmanned vehicle relative to the second unmanned vehicle; establishing a measurement matrix according to the distance information of the first unmanned vehicle relative to the second unmanned vehicle; and carrying out fusion positioning according to the state transition matrix and the measurement matrix to obtain an estimated coordinate of the first unmanned vehicle relative to the second unmanned vehicle. The positioning system includes: a first drone vehicle; a second drone vehicle; the first unmanned vehicle and the second unmanned vehicle are respectively provided with: an inertial navigation unit and an ultra-wideband wireless communication unit.)

1. A co-location method is applied to an unmanned vehicle group, the unmanned vehicle group at least comprises a first unmanned vehicle and a second unmanned vehicle, and the location method comprises the following steps:

establishing a state transition matrix according to the action information of the first unmanned vehicle relative to the second unmanned vehicle;

establishing a measurement matrix according to the distance information of the first unmanned vehicle relative to the second unmanned vehicle; the distance information of the first unmanned vehicle relative to the second unmanned vehicle is acquired through wireless communication tags respectively installed on the first unmanned vehicle and the second unmanned vehicle;

and carrying out fusion positioning according to the state transition matrix and the measurement matrix to obtain an estimated coordinate of the first unmanned vehicle relative to the second unmanned vehicle.

2. The positioning method of claim 1, wherein establishing a state transition matrix from motion information of the first drone vehicle relative to the second drone vehicle comprises:

establishing a state transition equation through the action information;

and establishing the state transition matrix according to the state transition equation.

3. The positioning method according to claim 2, wherein the action information includes: v. of_k、θ^yaw、ω_kAnd alpha_k；

Wherein establishing the state transition equation through the action information includes:

dividing observation points at time intervals, and performing iterative correction on each observation point according to action information;

the state transition equation is expressed by the following formula:

the expression (1) represents that the predicted value of the k +1 th time is obtained according to the prediction of the k result;

wherein p is_kRepresenting the position coordinates, v, of the first drone vehicle relative to the second drone vehicle_kRepresenting the speed, θ, of the first drone vehicle relative to the second drone vehicle^yawRepresenting the yaw angle, σ, of the first drone vehicle relative to the second drone vehicle_fRepresenting the zero offset, σ, of the accelerometer_wRepresenting zero bias, ω, of the gyroscope_kRepresenting an angular velocity of the first drone vehicle relative to the second drone vehicle;andrepresenting gaussian noise;

α_krepresenting an acceleration of the first drone vehicle relative to the second drone vehicle;

a is said_kCalculated using the formula:

a_k＝a_k ⁱ-a_k ^jformula (2)

a_k ⁱ＝Rⁱ _k(f_k-δ_f,k-w_a) -g formula (3)

In the formulae (2) and (3),a two-dimensional rotation matrix representing a rotation from a body coordinate system to a reference coordinate system of the drone vehicle, i represents the first droneAn ith wireless communication tag of the second drone vehicle, j represents a jth wireless communication tag of the first drone vehicle, f_kRepresenting the input of the accelerometer at the moment k, and g representing the acceleration of gravity;

representing the state transition equation in a matrix form as the state transition matrix,

the state transition matrix is represented by the following formula:

4. the positioning method of claim 1, wherein establishing a measurement matrix from the distance information of the first drone vehicle relative to the second drone vehicle comprises:

collecting the distance between each ultra-wideband wireless communication tag on the first unmanned vehicle and each ultra-wideband wireless communication tag on the second unmanned vehicle, using the collected distance as an observation element, generating a group of observation vectors by a plurality of observation elements, and establishing an observation equation according to the observation vectors;

the observation equation is used to constrain a distance of each of the ultra-wideband wireless communication tags on the first drone relative to each of the ultra-wideband wireless communication tags on the second drone.

5. The positioning method according to claim 4,

four ultra-wide wireless communication tags are respectively arranged on the first unmanned vehicle and the second unmanned vehicle,

respectively ranging each ultra-wideband wireless communication tag on the first unmanned vehicle relative to each ultra-wideband wireless communication tag on the second unmanned vehicle, collecting sixteen observation elements in total, and generating observation vectors according to the sixteen observation elements;

the observation equation established according to the observation vector is expressed by the following formula:

updating an observation vector based on the observation equation to establish the observation matrix, wherein the observation matrix is a Jacobian matrix and is represented by the following formula:

in the formulae (5) and (6),representing the observation element; p is a radical of_kRepresenting position coordinates of the first drone vehicle relative to the second drone vehicle,representing a two-dimensional rotation matrix of the mth unmanned vehicle from a body coordinate system to a reference system;representing a two-dimensional rotation matrix of the nth unmanned vehicle from a body coordinate system to a reference system;initial coordinates of the ith UWB wireless communication tag in the mth drone vehicle;initial coordinates of the jth ultra-wideband wireless communication tag in the nth drone vehicle;to observe noise; x is the number of_kRepresenting the position of the drone vehicle on the x-axis in a horizontal coordinate system, y_kIndicating the position of the y-axis of the unmanned vehicle in a coordinate system in the horizontal direction;

wherein the content of the first and second substances,andin order to advance the value of the calibration,depending on the parameters of the equipment used.

6. The positioning method according to claim 1, wherein performing fusion positioning according to the state transition matrix and the measurement matrix to obtain estimated coordinates of the first drone vehicle relative to the second drone vehicle comprises:

according to the last state X_kCalculating the predicted value of the current state, and adopting the following formula to represent:

in the formula (7), u_kInvolving an acceleration f_kAnd angular velocity ω_kCharacterizing the input at time k;

from the last state covariance matrix P_kCalculating a state covariance matrix between the predicted value and the true value of the current timeThe above-mentionedRepresented by the following formula:

wherein F represents the state transition matrix; q represents a covariance matrix of process noise, which is an artificial set value;

calculating the Kalman gain K of this time_k：

Wherein H represents a Jacobian matrix that maps the predicted state to the observation model; r is an observation noise matrix and is an artificial set value;

obtaining the current state quantity optimal estimation according to the observation model at this time:

wherein Z is_kA measurement value representing a relative distance of the wireless communication tag;

updating the state covariance matrix:

wherein, P_kIs the updated state covariance matrix, P_k＝(x_k,y_k) Characterizing estimated coordinates of the first drone relative to the second drone.

7. A co-location system, comprising:

a first drone vehicle;

a second drone vehicle;

the first unmanned vehicle and the second unmanned vehicle are respectively provided with:

the inertial navigation unit is used for acquiring action information of the first unmanned vehicle relative to the second unmanned vehicle; and

an ultra-wideband wireless communication unit for acquiring positional information of the first drone vehicle relative to the second drone vehicle; the ultra-wideband wireless communication unit includes: the position information is calculated through the distance between the ultra-wideband wireless communication tags on the first unmanned vehicle and the second unmanned vehicle, the ultra-wideband wireless communication tags on the same unmanned vehicle are uniformly arranged, a polygon is formed by taking the installation position of each ultra-wideband wireless communication tag as a vertex, and the center of the polygon is overlapped with the installation position of the inertial navigation unit.

8. The positioning system of claim 7, wherein the inertial navigation unit comprises at least:

an accelerometer to obtain relative accelerations of the first and second drone vehicles; and

a gyroscope to obtain relative angular velocities of the first and second unmanned vehicles.

9. The positioning system of claim 7, wherein the ultra-wideband wireless communication unit comprises:

at least four of the ultra-wideband wireless communication tags.

10. The location system of claim 9, wherein the number of ultra-wideband wireless communication tags is four, wherein,

four point locations are uniformly distributed on the first unmanned vehicle and the second unmanned vehicle respectively, the four point locations on the same unmanned vehicle are arranged at intervals on four vertexes of a square formed in a horizontal plane, and one ultra-wideband wireless communication tag is installed on each point location.

Technical Field

The invention relates to the technical field of unmanned vehicle positioning, in particular to a cooperative positioning method and a positioning system.

Background

With the development of mobile vehicles such as unmanned aerial vehicles and unmanned vehicles, the cluster cooperative positioning technology is increasingly concerned at home and abroad. The cluster cooperation technology has the advantages of large working range, high reliability, capability of simultaneously executing multiple tasks, high overall efficiency and the like, and can be used in the fields of disaster exploration, rescue and the like. Different from single-point positioning, cluster positioning requires that a plurality of nodes to be positioned independently complete positioning tasks of all nodes only depending on information acquired by sensors carried by the nodes in a system under the working conditions of no prior information (map information and the like) and no external facility assistance (communication base stations and the like).

Currently, the cooperative positioning of the unmanned mobile vehicles mainly depends on a satellite signal differential positioning (GNSS-RTK) mode, which not only requires that fixed geometric shapes are kept between the unmanned mobile vehicles, but also depends on high-precision RTK hardware with high cost; besides the technical approach relying on satellite signals, other cluster positioning technologies include a master-slave relative positioning scheme, a cooperative positioning scheme based on machine vision, and the like. However, such solutions have their inherent drawbacks in practical applications: the master-slave relative positioning requires that host nodes are predefined among all nodes, and when the host nodes have communication faults, the whole cluster positioning system is paralyzed, so that the safety is low; the cooperative positioning based on machine vision is restricted by the field of view of a camera, the problem that a moving target is shielded cannot be solved, and the positioning precision is greatly reduced due to the fact that the moving target is easily interfered in a dynamic environment.

Disclosure of Invention

Technical problem to be solved

The present invention provides a co-location method and a co-location system for solving at least some of the above technical problems.

(II) technical scheme

The invention discloses a cooperative positioning method which is applied to an unmanned vehicle group, wherein the unmanned vehicle group at least comprises a first unmanned vehicle and a second unmanned vehicle, and the positioning method comprises the following steps:

establishing a state transition matrix according to the action information of the first unmanned vehicle relative to the second unmanned vehicle; establishing a measurement matrix according to the distance information of the first unmanned vehicle relative to the second unmanned vehicle; the distance information of the first unmanned vehicle relative to the second unmanned vehicle is acquired through wireless communication tags respectively installed on the first unmanned vehicle and the second unmanned vehicle; and carrying out fusion positioning according to the state transition matrix and the measurement matrix to obtain an estimated coordinate of the first unmanned vehicle relative to the second unmanned vehicle.

According to an embodiment of the present disclosure, establishing a state transition matrix according to motion information of the first drone vehicle relative to the second drone vehicle includes:

establishing a state transition equation through the action information; and establishing the state transition matrix according to the state transition equation.

According to an embodiment of the present disclosure, the action information includes:

v_k、θ^yaw、ω_kand alpha_k；

Wherein establishing the state transition equation through the action information includes: dividing observation points at time intervals, and performing iterative correction on each observation point according to action information;

the state transition equation is expressed by the following formula:

the expression (1) represents that the predicted value of the k +1 th time is obtained according to the prediction of the k result;

wherein p is_kRepresenting the position coordinates, v, of the first drone vehicle relative to the second drone vehicle_kRepresenting the speed, θ, of the first drone vehicle relative to the second drone vehicle^yawRepresenting the yaw angle, σ, of the first drone vehicle relative to the second drone vehicle_fRepresenting the zero offset, σ, of the accelerometer_wRepresenting zero bias, ω, of the gyroscope_kRepresenting an angular velocity of the first drone vehicle relative to the second drone vehicle;andrepresenting gaussian noise;

α_krepresenting an acceleration of the first drone vehicle relative to the second drone vehicle; a is said_kCalculated using the formula:

a_k＝a_k ⁱ-a_k ^jformula (2)

a_k ⁱ＝Rⁱ _k(f_k-δ_f,k-w_a) -g formula (3)

In the formulae (2) and (3),a two-dimensional rotation matrix representing a body coordinate system to a reference coordinate system of the drone vehicle, i represents an ith wireless communication tag of the first drone vehicle, j represents a jth wireless communication tag of the second drone vehicle, f_kRepresenting the input of the accelerometer at the moment k, and g representing the acceleration of gravity; representing the state transition equation into the state transition matrix in a matrix form, wherein the state transition matrix is represented by the following formula:

according to an embodiment of the present disclosure, establishing a measurement matrix from distance information of the first drone vehicle relative to the second drone vehicle includes: collecting the distance between each ultra-wideband wireless communication tag on the first unmanned vehicle and each ultra-wideband wireless communication tag on the second unmanned vehicle, using the collected distance as an observation element, generating a group of observation vectors by a plurality of observation elements, and establishing an observation equation according to the observation vectors;

the observation equation is used to constrain a distance of each of the ultra-wideband wireless communication tags on the first drone relative to each of the ultra-wideband wireless communication tags on the second drone.

According to an embodiment of the disclosure, four ultra-wide wireless communication tags are respectively installed on the first unmanned vehicle and the second unmanned vehicle, each ultra-wide wireless communication tag on the first unmanned vehicle is respectively subjected to ranging relative to each ultra-wide wireless communication tag on the second unmanned vehicle, sixteen observation elements are collected in total, and observation vectors are generated according to the sixteen observation elements; the observation equation established according to the observation vector is expressed by the following formula:

updating an observation vector based on the observation equation to establish the observation matrix, wherein the observation matrix is a Jacobian matrix and is represented by the following formula:

in the formulae (5) and (6),representing the observation element; p is a radical of_kRepresenting position coordinates of the first drone vehicle relative to the second drone vehicle,representing a two-dimensional rotation matrix of the mth unmanned vehicle from a body coordinate system to a reference system;representing a two-dimensional rotation matrix of the nth unmanned vehicle from a body coordinate system to a reference system;the ith ultra-wideband antenna in the mth unmanned vehicleInitial coordinates of the line communication tag;initial coordinates of the jth ultra-wideband wireless communication tag in the nth drone vehicle;to observe noise; x is the number of_kRepresenting the position of the drone vehicle on the x-axis in a horizontal coordinate system, y_kIndicating the position of the y-axis of the unmanned vehicle in a coordinate system in the horizontal direction; wherein the content of the first and second substances,andin order to advance the value of the calibration,depending on the parameters of the equipment used.

According to an embodiment of the present disclosure, performing fusion positioning according to the state transition matrix and the measurement matrix to obtain estimated coordinates of the first drone vehicle relative to the second drone vehicle includes:

according to the last state X_kCalculating the predicted value of the current state, and adopting the following formula to represent:

in the formula (7), u_kInvolving an acceleration f_kAnd angular velocity ω_kCharacterizing the input at time k;

from the last state covariance matrix P_kCalculating a state covariance matrix between the predicted value and the true value of the current timeThe above-mentionedRepresented by the following formula:

wherein F represents the state transition matrix; q represents a covariance matrix of process noise, which is an artificial set value;

calculating the Kalman gain K of this time_k：

Wherein H represents a Jacobian matrix that maps the predicted state to the observation model; r is an observation noise matrix and is an artificial set value;

obtaining the current state quantity optimal estimation according to the observation model at this time:

wherein Z is_kA measurement value representing a relative distance of the wireless communication tag;

updating the state covariance matrix:

wherein, P_kIs the updated state covariance matrix, P_k＝(x_k,y_k) Characterizing estimated coordinates of the first drone relative to the second drone.

As another aspect of the present invention, there is also exemplarily disclosed a co-location system including:

a first drone vehicle; a second drone vehicle, the first drone vehicle and the second drone vehicle having mounted thereon, respectively: the inertial navigation unit is used for acquiring action information of the first unmanned vehicle relative to the second unmanned vehicle; and an ultra-wideband wireless communication unit for obtaining positional information of the first drone vehicle relative to the second drone vehicle; the ultra-wideband wireless communication unit comprises a plurality of ultra-wideband wireless communication tags, the position information is calculated according to the distance between the ultra-wideband wireless communication tags on the first unmanned vehicle and the second unmanned vehicle, the ultra-wideband wireless communication tags on the same unmanned vehicle are uniformly arranged, each ultra-wideband wireless communication tag is arranged at the position as a vertex to form a polygon, and the center of the polygon is overlapped with the mounting position of the inertial navigation unit.

According to an embodiment of the present disclosure, the inertial navigation unit comprises at least:

an accelerometer to obtain relative accelerations of the first and second drone vehicles; and a gyroscope to obtain relative angular velocities of the first and second unmanned vehicles.

According to an embodiment of the present disclosure, the ultra-wideband wireless communication unit includes:

at least four of the ultra-wideband wireless communication tags.

According to an embodiment of the present disclosure, the number of the ultra-wideband wireless communication tags is four, wherein,

four point locations are uniformly distributed on the first unmanned vehicle and the second unmanned vehicle respectively, the four point locations on the same unmanned vehicle are arranged at intervals on four vertexes of a square formed in a horizontal plane, and one ultra-wideband wireless communication tag is installed on each point location.

(III) advantageous effects

The invention provides a cooperative positioning system which comprises an ultra-wideband wireless communication unit and an inertial navigation unit. The ultra-wideband wireless communication unit is used for acquiring distance information of two unmanned vehicles, and the inertial navigation unit is used for acquiring action information of the two unmanned vehicles. And performing fusion positioning based on the distance information and the action information to further obtain the coordinate information of one unmanned vehicle relative to the other unmanned vehicle. When a certain unmanned vehicle in the unmanned vehicle group breaks down, other unmanned vehicles can still normally work through mutual cooperative positioning. Compared with the prior art, the cooperative positioning system is higher in safety and higher in reliability.

The invention also provides a cooperative positioning method, which is based on the cooperation of the ultra-wideband wireless communication unit and the inertial navigation unit, and compared with the high-precision RTK adopted in the prior art, the method supports normal work in the environment where GNSS is rejected (such as indoor, underground or other GNSS rejection working conditions), and has lower cost. Compared with other cluster positioning technologies in the prior art, the method does not need a fixed base station, so that the range of a positioning scene is greatly widened, and the direction information of the mobile node can be observed after the inertial navigation unit is added; there is no visual field limitation and no dependency on the environment texture.

Drawings

Fig. 1 is a flow chart of a co-location method according to an exemplary embodiment of the present invention;

FIG. 2 is a schematic diagram of a co-location system in accordance with an exemplary embodiment of the present invention;

FIG. 3 is a schematic illustration of the relative trajectories of a cluster of unmanned vehicles to which an exemplary embodiment of the location method of the present invention is applied;

FIGS. 4a-4e are graphs comparing true and estimated values in the embodiment of FIG. 3, where FIG. 4a is a graph comparing UGV2 with UGV1 yaw angle, FIG. 4b is a graph comparing UGV2 with UGV 1X axial velocity, FIG. 4c is a graph comparing UGV2 with UGV 1Y axial velocity, FIG. 4d is a graph comparing UGV2 with UGV 1X axial displacement, and FIG. 4e is a graph comparing UGV2 with UGV 1Y axial displacement;

FIG. 5 is a schematic diagram of the relative trajectories of a cluster of unmanned vehicles to which another exemplary embodiment of the location method of the present invention is applied;

fig. 6 is a schematic diagram of the relative positions of the unmanned vehicle cluster in the embodiment of fig. 5.

Detailed Description

In order that the objects, technical solutions and advantages of the present invention will become more apparent, the present invention will be further described in detail with reference to the accompanying drawings in conjunction with the following specific embodiments.

The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the disclosure. The terms "comprises," "comprising," and the like, as used herein, specify the presence of stated features, steps, operations, and/or components, but do not preclude the presence or addition of one or more other features, steps, operations, or components.

All terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art unless otherwise defined. It is noted that the terms used herein should be interpreted as having a meaning that is consistent with the context of this specification and should not be interpreted in an idealized or overly formal sense.

Where a convention analogous to "at least one of A, B and C, etc." is used, in general such a construction is intended in the sense one having skill in the art would understand the convention (e.g., "a system having at least one of A, B and C" would include but not be limited to systems that have a alone, B alone, C alone, a and B together, a and C together, B and C together, and/or A, B, C together, etc.). Where a convention analogous to "A, B or at least one of C, etc." is used, in general such a construction is intended in the sense one having skill in the art would understand the convention (e.g., "a system having at least one of A, B or C" would include but not be limited to systems that have a alone, B alone, C alone, a and B together, a and C together, B and C together, and/or A, B, C together, etc.).

The invention provides a cooperative positioning method, which is applied to an unmanned vehicle group, wherein the unmanned vehicle group at least comprises a first unmanned vehicle and a second unmanned vehicle.

The positioning method comprises the following steps:

establishing a state transition matrix according to the action information of the first unmanned vehicle relative to the second unmanned vehicle;

establishing a measurement matrix according to the distance information of the first unmanned vehicle relative to the second unmanned vehicle; the distance information of the first unmanned vehicle relative to the second unmanned vehicle is acquired through wireless communication tags respectively installed on the first unmanned vehicle and the second unmanned vehicle;

and carrying out fusion positioning according to the state transition matrix and the measurement matrix to obtain an estimated coordinate of the first unmanned vehicle relative to the second unmanned vehicle.

Fig. 1 is a flowchart of a co-location method according to an exemplary embodiment of the present invention.

In one exemplary embodiment, as shown in FIG. 1: acquiring action information of the first unmanned vehicle relative to the second unmanned vehicle through inertial navigation units (IMUs) respectively mounted on the first unmanned vehicle and the second unmanned vehicle; after the action information is collected, the inertial navigation unit data needs to be preprocessed, and a state transition matrix is established.

In one exemplary embodiment, as shown in FIG. 1: the distance information of the first unmanned vehicle relative to the second unmanned vehicle is collected through a plurality of ultra wide band wireless communication tags in ultra wide band wireless communication Units (UWB) respectively mounted on the first unmanned vehicle and the second unmanned vehicle; after the distance information is collected, the data of the ultra-wideband wireless communication unit needs to be preprocessed, and a measurement matrix is established.

In one exemplary embodiment, as shown in FIG. 1: performing fusion positioning according to the state transition matrix and the measurement matrix, including: initializing EKF parameters; trilateral localization least squares solution; state X, covariance P prediction; calculating a Kalman gain K; and updating the covariance P according to the state X to obtain estimated coordinates of the first drone vehicle relative to the second drone vehicle.

In one exemplary embodiment, establishing a state transition matrix from motion information of a first drone vehicle relative to a second drone vehicle includes:

establishing a state transition equation through the action information;

and establishing a state transition matrix according to the state transition equation.

In one exemplary embodiment, the action information includes: v. of_k、θ^yaw、ω_kAnd alpha_k；

Wherein, establish the state transition equation through the action information, include:

dividing observation points at time intervals, and performing iterative correction on each observation point according to the action information;

the state transition equation is expressed by the following formula:

the expression (1) represents that the predicted value of the k +1 th time is obtained according to the prediction of the k result;

wherein p is_kRepresenting the position coordinates, v, of the first drone vehicle relative to the second drone vehicle_kRepresenting the velocity, θ, of the first drone vehicle relative to the second drone vehicle^yawRepresenting the yaw angle, σ, of the first drone vehicle relative to the second drone vehicle_fRepresenting the zero offset, σ, of the accelerometer_wRepresenting zero bias, ω, of the gyroscope_kRepresenting an angular velocity of the first drone vehicle relative to the second drone vehicle;andrepresenting gaussian noise;

α_krepresenting an acceleration of the first drone vehicle relative to the second drone vehicle;

α_kcalculated using the formula:

a_k＝a_k ⁱ-a_k ^jformula (2)

a_k ⁱ＝Rⁱ _k(f_k-δ_f,k-w_a) -g formula (3)

In the formulae (2) and (3),representing a two-dimensional rotation matrix from a body coordinate system of the drone vehicle to a reference coordinate system, i represents an ith wireless communication tag of the first drone vehicle, j represents a jth wireless communication tag of the second drone vehicle, f_kRepresenting the input of the accelerometer at the moment k, and g representing the acceleration of gravity;

the state transition equations are expressed in matrix form as a state transition matrix,

a state transition matrix represented by the following equation:

in one exemplary embodiment, establishing a measurement matrix from distance information of a first drone vehicle relative to a second drone vehicle includes:

collecting the distance between each ultra-wideband wireless communication tag on the first unmanned vehicle and each ultra-wideband wireless communication tag on the second unmanned vehicle, using the collected distance as an observation element, generating a group of observation vectors by a plurality of observation elements, and establishing an observation equation according to the observation vectors;

the observation equation is used to constrain the distance of each ultra-wideband wireless communication tag on the first drone vehicle relative to each ultra-wideband wireless communication tag on the second drone vehicle.

In one exemplary embodiment, four ultra-wide wireless communication tags are mounted on each of the first and second drone vehicles,

respectively ranging each ultra-wideband wireless communication tag on the first unmanned vehicle relative to each ultra-wideband wireless communication tag on the second unmanned vehicle, collecting sixteen observation elements in total, and generating observation vectors according to the sixteen observation elements;

and (3) an observation equation established according to the observation vector is represented by the following formula:

updating an observation vector based on an observation equation to establish an observation matrix, wherein the observation matrix is a Jacobian matrix and is represented by the following formula:

in the formulae (5) and (6),representing an observation element; p is a radical of_kRepresenting the position coordinates of the first drone vehicle relative to the second drone vehicle,representing a two-dimensional rotation matrix of the mth unmanned vehicle from a body coordinate system to a reference system;representing a two-dimensional rotation matrix of the nth unmanned vehicle from a body coordinate system to a reference system;initial coordinates of an ith ultra-wideband wireless communication tag in the mth unmanned vehicle;initial coordinates of a jth ultra-wideband wireless communication tag in an nth unmanned vehicle;to observe noise; x is the number of_kIndicating the position of the x-axis of the drone vehicle in a horizontal coordinate system, y_kTo representThe position of the unmanned vehicle on the y axis in a horizontal coordinate system;

wherein the content of the first and second substances,andin order to advance the value of the calibration,depending on the parameters of the equipment used.

In one exemplary embodiment, performing fusion positioning based on the state transition matrix and the measurement matrix to obtain estimated coordinates of the first drone vehicle relative to the second drone vehicle includes:

initializing the EKF parameters by taking the relative initial speed as 0 as a state initial value of an EKF algorithm;

wherein the initial position is p solved by trilateral positioning algorithm₀。

According to the last state X_kCalculating the predicted value of the current state, and adopting the following formula to represent:

in the formula (7), u_kInvolving an acceleration f_kAnd angular velocity ω_kCharacterizing the input at time k;

from the last state covariance matrix P_kCalculating a state covariance matrix between the predicted value and the true value of the current timeRepresented by the following formula:

wherein F represents a state transition matrix; q represents a covariance matrix of process noise, which is an artificial set value;

calculating the Kalman gain K of this time_k：

Wherein H represents a Jacobian matrix that maps the predicted state to the observation model; r is an observation noise matrix and is an artificial set value;

obtaining the current state quantity optimal estimation according to the observation model at this time:

wherein Z is_kA measurement value representing a relative distance of the wireless communication tag;

updating the state covariance matrix:

P_k＝P_k ^-[I-K_kH_k]formula (11)

Wherein, P_kIs the updated state covariance matrix, P_k＝(x_k,y_k) An estimated coordinate characterizing a relative between the first drone vehicle relative to the second drone vehicle.

Fig. 2 is a schematic structural diagram of a co-location system according to an exemplary embodiment of the present invention.

A co-location system, as shown in fig. 2, comprising:

a first drone vehicle 3;

second unmanned vehicle 4 installs respectively on first unmanned vehicle and the second unmanned vehicle:

an inertial navigation unit 1 for acquiring motion information (T) of a first unmanned vehicle relative to a second unmanned vehicle_ijCharacterizing motion information of the ith drone vehicle relative to the jth drone vehicle).

An ultra-wideband wireless communication unit 2 for acquiring a first unmanned vehicle phasePosition information (T) for a second drone vehicle^ijA transformation matrix characterizing the body coordinate system of the ith drone vehicle relative to the jth drone vehicle).

The ultra-wideband wireless communication unit comprises a plurality of ultra-wideband wireless communication tags, position information is calculated through the distance between the ultra-wideband wireless communication tags on the first unmanned vehicle and the second unmanned vehicle, the ultra-wideband wireless communication tags on the same unmanned vehicle are uniformly arranged, a polygon is formed by taking the installation position of each ultra-wideband wireless communication tag as a vertex, and the center of the polygon is overlapped with the installation position of the inertial navigation unit.

In an exemplary embodiment, the distance between two adjacent ultra-wideband wireless communication tags should be no less than 1 meter. By adopting the embodiment, the positioning accuracy of the ultra-wideband wireless communication unit is improved.

In an exemplary embodiment, the inertial navigation unit comprises at least:

an accelerometer for acquiring relative acceleration of the first drone vehicle and the second drone vehicle; and a gyroscope for acquiring relative angular velocities of the first and second unmanned vehicles.

In one exemplary embodiment, an ultra-wideband wireless communication unit, comprises: at least four ultra-wideband wireless communication tags.

In an exemplary embodiment, the number of the ultra-wideband wireless communication tags is four, wherein four point locations are uniformly distributed on the first unmanned vehicle and the second unmanned vehicle respectively, the four point locations on the same unmanned vehicle are arranged at intervals on a horizontal plane to form four vertexes of a square, and each point location is provided with one ultra-wideband wireless communication tag.

In an exemplary embodiment, the inertial navigation unit should be arranged at the center position of the square.

In an exemplary embodiment, the four wireless communication tags are installed at one corner of the unmanned vehicle, and the four wireless communication tags are located at four vertexes of a square with the side length of 1 meter. The distance measurement error of the wireless communication tag (UWB) is 0.1 meter, the angular velocity zero offset of the inertial navigation unit (IMU) is 0.01, the acceleration zero offset is 0.01, and the noise of the inertial navigation unit is white Gaussian noise with the standard deviation of 0.01.

Static experiments and dynamic experiments are used as an exemplary embodiment, respectively.

Static state experiment:

the geometric center of the UGV1 coincides with the origin point, with the initial position of the first unmanned vehicle (UGV1), and the UGV1 remains stationary. The second drone vehicle (UGV2) makes a uniform circular motion around UGV1, with a radius of the circle of 10 meters and an angular velocity of 0.01 pi.

Fig. 3 is a diagram of the relative trajectories of a cluster of unmanned vehicles to which an exemplary embodiment of the localization method of the present invention is applied.

As shown in fig. 3, an XY coordinate system is established with the initial position of UGV1 as the origin of coordinates. The trajectory true 6, EKF estimated true 5, and trilateration solution 7 during the UGV2 move are shown in the figure.

FIGS. 4a-4e are graphs comparing the true value and the EKF estimate true value in the embodiment of FIG. 3.

Based on the static experiments described above, a trajectory range of 100 meters was intercepted, as shown in fig. 4a-4 e.

As shown in FIG. 4a, a coordinate system is established with a trajectory of 100 meters as the horizontal axis and the UGV2 as the vertical axis with respect to the yaw angle of the UGV 1. Travel is approximately one turn around the UGV1 with 100 meters of UGV2 travel. The trace truth 5 and the EKF estimate truth 6 are compared in a coordinate system.

As shown in fig. 4b, the trace true value 5 and the EKF estimated true value 6 are compared in a coordinate system with the trace of 100 meters as the horizontal axis and the displacement of the UGV2 with respect to the UGV1 in the X direction in fig. 3 as the vertical axis.

As shown in fig. 4c, the trajectory true value 5 and the EKF estimated true value 6 are compared in a coordinate system with the trajectory of 100 meters as the horizontal axis and the displacement of the UGV2 with respect to the UGV1 in the Y direction in fig. 3 as the vertical axis.

As shown in fig. 4d, the true value and the EKF estimate true value are displayed and compared in a coordinate system with a trajectory of 100 meters as the horizontal axis and the speed of UGV2 with respect to UGV1 in the X direction in fig. 3 as the vertical axis.

As shown in fig. 4e, the trace true value 5 and the EKF estimated true value 6 are compared in a coordinate system with the trace of 100 meters as the horizontal axis and the speed of the UGV2 with respect to the UGV1 in the Y direction in fig. 3 as the vertical axis.

From the above descriptions of fig. 4a to 4e, it can be seen that the trajectory error (RMSE) after EKF optimization is 0.08m, and the trajectory error of trilateral localization is 0.24 m. The longitude is better than the GPS average positioning accuracy.

Dynamic experiment: the initial position of the first unmanned vehicle (UGV1) is used as an origin, the geometric center of the UGV1 is coincident with the origin, and the UGV1 moves in a uniform linear motion in the positive direction of the X axis of the coordinate. The second drone vehicle (UGV2) makes a uniform circular motion around UGV1, with a radius of the circle of 10 meters and an angular velocity of 0.01 pi.

Fig. 5 is a schematic diagram of the relative trajectories of a cluster of unmanned vehicles to which another exemplary embodiment of the location method of the present invention is applied.

As shown in fig. 5, the first drone vehicle has a movement trajectory as indicated by reference numeral 8 and the second drone vehicle has a movement trajectory as indicated by reference numeral 9. And establishing an XY coordinate system by taking the middle point moved by the UGV1 as a coordinate origin. UGV1 moves from the-5 position to the +5 position on the X-axis, and UGV2 runs one week around UGV 1.

Fig. 6 is a schematic diagram of the relative positions of the unmanned vehicle cluster in the embodiment of fig. 5.

As shown in FIG. 6, the trajectory true value, EKF estimated true value, and trilateration solution during UGV2 movement are shown and compared in the figure.

Under the condition that the UGV1 and the UGV2 both move, the track error (RMSE) of the optimized relative position is 0.38m, which is still better than the average precision of GPS positioning.

It can be known that the cooperative positioning method related in the embodiment of the present disclosure can meet the corresponding precision requirement.

The embodiments of the present disclosure have been described above. However, these examples are for illustrative purposes only and are not intended to limit the scope of the present disclosure. Although the embodiments are described separately above, this does not mean that the measures in the embodiments cannot be used in advantageous combination. The scope of the disclosure is defined by the appended claims and equivalents thereof. Various alternatives and modifications can be devised by those skilled in the art without departing from the scope of the present disclosure, and such alternatives and modifications are intended to be within the scope of the present disclosure.