Air-ground unmanned system cooperative positioning method

Abstract

The application discloses a kind of ground-air unmanned system cooperative positioning method, comprising: determining the kinematic state equation of slave unmanned vehicle;Obtain the state transition equation of slave unmanned vehicle;Determine the measurement equation of master unmanned vehicle and slave unmanned vehicle;Obtain linearized model;According to linearized model, state prediction and update are carried out;Determine the positioning result of master unmanned vehicle and slave unmanned vehicle;According to the positioning result, the distance between unmanned aerial vehicle and master unmanned vehicle and the distance between unmanned aerial vehicle and slave unmanned vehicle are calculated;According to the distance calculated, real-time wireless positioning is carried out to unmanned aerial vehicle, and the position coordinates of unmanned aerial vehicle are obtained;Determine state variable according to unmanned aerial vehicle motion model;The differential form of inertial navigation system state equation is constructed;Determine the measurement equation based on multi-source sensor;According to the differential form of inertial navigation system state equation and the measurement equation based on multi-source sensor, the position coordinates of unmanned aerial vehicle are error corrected by Kalman filter, and the corrected position coordinates are obtained.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of positioning and navigation, in particular to a method for cooperative positioning of a ground-air unmanned system. BACKGROUND

[0002] The ground-air unmanned system composed of unmanned vehicles and unmanned aerial vehicles can realize the complementation in sensing, navigation, load, communication and other aspects through mutual cooperation, improve the flexibility and adaptability of the whole system to unknown environment, and thus complete the tasks that the homogeneous unmanned vehicle formation or unmanned aerial vehicle formation cannot complete, such as rescue search, regional investigation, geographic survey, map drawing and tracking and pursuit.

[0003] Satellite navigation technology is an effective means to support the autonomous movement of unmanned systems, but it is easily affected by complex factors such as space obstruction, electromagnetic interference and deception, which restricts the application of the ground-air cooperative unmanned system. The positioning method based on laser point cloud and image is greatly affected by the environment image, and in the scene where the moving speed is fast, the geometric / texture features of the environment are not obvious, there are many dynamic objects, or the light / reflection level changes greatly, it is easy to produce degradation or even failure. As a passive navigation technology, inertial technology is less affected by external environmental factors, but there is cumulative error in long-time work. The active wireless communication technology represented by ultra-wideband technology can be used for local distance measurement, but the ranging error is large in dynamic scenes, which is not suitable for independent use. SUMMARY

[0004] The present application provides a method for cooperative positioning of a ground-air unmanned system, which can solve the problems in the prior art.

[0005] The present application provides a method for cooperative positioning of a ground-air unmanned system, wherein the ground-air unmanned system comprises a master unmanned vehicle, a slave unmanned vehicle and an unmanned aerial vehicle, and the method comprises:

[0006] determining a kinematic state equation of the slave unmanned vehicle;

[0007] obtaining a state transition equation of the slave unmanned vehicle according to the kinematic state equation and a Gaussian distribution of sensor noise;

[0008] determining a measurement equation of the master unmanned vehicle and the slave unmanned vehicle;

[0009] linearizing the system by using a Kalman filtering method of a nonlinear system according to the state transition equation and the measurement equation, to obtain a linearized model;

[0010] performing state prediction and updating of the master-slave unmanned vehicles according to the linearized model, to obtain state prediction values and updating values;

[0011] determining the positioning results of the master unmanned vehicle and the slave unmanned vehicle according to the state prediction values and the updating values;

[0012] According to the positioning result, the distance between the unmanned aerial vehicle and the master unmanned vehicle and the distance between the unmanned aerial vehicle and the slave unmanned vehicle are calculated;

[0013] The least square method is adopted to perform real-time wireless positioning on the unmanned aerial vehicle according to the calculated distance, so as to obtain the position coordinates of the unmanned aerial vehicle;

[0014] The state variable is determined according to the motion model of the unmanned aerial vehicle;

[0015] The differential form of the inertial navigation system state equation is constructed based on the error model of the inertial navigation system;

[0016] The measurement equation based on the multi-source sensor is determined;

[0017] According to the differential form of the inertial navigation system state equation and the measurement equation based on the multi-source sensor, the error of the position coordinates of the unmanned aerial vehicle is corrected by the Kalman filter, so as to obtain the corrected position coordinates.

[0018] Preferably, the kinematic state equation of the slave unmanned vehicle is:

[0019]

[0020] wherein, are the lateral position coordinates, the longitudinal position coordinates and the heading angle of the unmanned vehicle at the i th moment respectively, are the lateral position coordinates, the longitudinal position coordinates and the heading angle of the unmanned vehicle at the i-1 th moment respectively, Δt is the sampling period, v i-1 is the linear velocity of the slave unmanned vehicle at the i-1 th moment, ω i is the angular velocity of the slave unmanned vehicle at the i-1 th moment.

[0021] Preferably, the state transition equation of the slave unmanned vehicle is:

[0022]

[0023] wherein, the superscript T is the transpose symbol, u i-1 is the real velocity of the slave unmanned vehicle at the i-1 th moment, u m(i-1) is the measured velocity of the slave unmanned vehicle at the i-1 th moment, w i-1 is the sensor noise of the Gaussian distribution at the i-1 th moment.

[0024] Preferably, the measurement equation of the master unmanned vehicle and the slave unmanned vehicle is:

[0025]

[0026] wherein, Z i is the distance measurement value of the master unmanned vehicle and the slave unmanned vehicle, V i is the measurement noise, h(X i ) is the observation matrix, x ai,y ai Let x and y be the horizontal and vertical coordinates of the autonomous vehicle at time i, respectively.

[0027] Preferably, obtaining a linearized model includes:

[0028] Wrap f(·) around X i-1 posterior value Expanding, neglecting terms of order two and above in the Taylor series, we get:

[0029]

[0030] The nonlinear function h(·) is wrapped around X. i Prior values Expanding, neglecting terms of order two and above in the Taylor series, we get:

[0031]

[0032] Where the first derivative H of the above equation i for:

[0033]

[0034] Preferably, the predicted state value and the updated state value are obtained by the following formula:

[0035]

[0036] in, P is the predicted state value. i / i-1 K represents the predicted covariance value. i Here are the Kalman filter coefficients, and I is the identity matrix. For the state update value, P i Let Q be the covariance matrix. i-1 Let be the covariance matrix of the process noise at time i-1.

[0037] Preferably, the distance between the drone and the master unmanned vehicle, and the distance between the drone and the slave unmanned vehicle, are calculated using the following formula:

[0038]

[0039] A·X uav =B,

[0040]

[0041] Where r1 is the distance between the drone and the master unmanned vehicle, r2 is the distance between the drone and the first slave unmanned vehicle, r3 is the distance between the drone and the second slave unmanned vehicle, and X uav =(x uav ,y uav Zuav ) is the position coordinate of the unmanned aerial vehicle, X ugv1 = (x1, y1, z1) is the position coordinate of the master unmanned vehicle, X ugv2 = (x2, y2, z2) is the position coordinate of the first slave unmanned vehicle, X ugv3 = (x3, y3, z3) is the position coordinate of the second slave unmanned vehicle.

[0042] Preferably, the position coordinate of the unmanned aerial vehicle is obtained by the following formula:

[0043] X uav_i = (A i T ·A i ) -1 ·A i T ·B i ,

[0044] Wherein, X uav_i is the position coordinate of the unmanned aerial vehicle at time i.

[0045] Preferably, the differential form of the inertial navigation system state equation is constructed based on the inertial navigation system error model:

[0046]

[0047] Wherein, F(t) is the system state transition matrix, X(t) is the system error state to be estimated, G(t) is the system noise transition matrix, and W(t) is the white noise error of the IMU.

[0048] Preferably, the measurement equation based on multi-source sensors is:

[0049] Z(t) = H·X(t) + V(t),

[0050] Wherein, H is the observation matrix, X(t) is the inertial navigation system state equation, and V(t) is the measurement noise.

[0051] Through the above technical solution, in the face of satellite navigation denial scenario, the wireless positioning technology can be combined with the dead reckoning technology, etc. The problem of inapplicability and unreliability of traditional positioning technology in the application scene of ground-air cooperative unmanned system formation is solved, and the cooperative positioning of ground-air unmanned system formation under the condition of not relying on satellite positioning is supported. BRIEF DESCRIPTION OF DRAWINGS

[0052] The accompanying drawings, which are included to provide a further understanding of the embodiments of the application and are incorporated in and constitute a part of this application, illustrate embodiments of the application and together with the description serve to explain the principles of the application. It is readily understood that the drawings are merely illustrative of some embodiments of the application and therefore are not to be construed as limiting the scope of the application as described herein.

[0053] Figure 1 Fig. 1 shows a master-slave unmanned vehicle formation positioning scene schematic diagram according to an embodiment of the present application;

[0054] Figure 2 Fig. 2 shows a ground-air unmanned system formation cooperative positioning scene schematic diagram according to an embodiment of the present application. DETAILED DESCRIPTION

[0055] It should be noted that the embodiments and the features in the embodiments in the present application can be combined with each other without conflict. The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings of the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. The description of the at least one exemplary embodiment is actually only illustrative, but not intended to limit the present application and its application or use in any way. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of the present application.

[0056] It should be noted that the terms used herein are only intended to describe specific embodiments, and are not intended to limit the exemplary embodiments according to the present application. As used herein, the singular form is intended to include the plural form unless the context clearly indicates otherwise, and it should also be understood that when the terms "comprise" and / or "include" are used in the specification, there is a feature, step, operation, device, component and / or combination thereof.

[0057] Unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of the invention. It should also be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale. Techniques, methods, and devices known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the specification. In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters in the following figures denote similar items; therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0058] This invention provides a cooperative positioning method for an air-to-ground unmanned system, wherein the air-to-ground unmanned system includes a master unmanned vehicle, slave unmanned vehicles, and unmanned aerial vehicles (UAVs), and the method includes:

[0059] Determine the kinematic state equations of the autonomous vehicle;

[0060] The state transition equation from the autonomous vehicle is obtained based on the kinematic state equation and the Gaussian distribution of the sensor noise.

[0061] Determine the measurement equations for the master and slave unmanned vehicles;

[0062] Based on the state transition equation and measurement equation, the Kalman filter method for nonlinear systems is used to linearize the system and obtain a linearized model.

[0063] Based on the linear model, the state prediction and update of the master-slave autonomous vehicle are performed to obtain the state prediction value and the updated value;

[0064] The positioning results of the master and slave autonomous vehicles are determined based on the predicted and updated state values.

[0065] Based on the positioning results, calculate the distance between the drone and the master unmanned vehicle, as well as the distance between the drone and the slave unmanned vehicle;

[0066] The least squares method is used to perform real-time wireless positioning of the UAV based on the calculated distance, and the position coordinates of the UAV are obtained.

[0067] Determine the state variables based on the UAV motion model;

[0068] The differential form of the state equation of the inertial navigation system is constructed based on the error model of the inertial navigation system.

[0069] Determine the measurement equations based on multi-source sensors;

[0070] According to the differential form of the inertial navigation system state equation and the measurement equation based on the multi-source sensor, the position coordinates of the unmanned aerial vehicle are error-corrected by a Kalman filter to obtain corrected position coordinates.

[0071] By the technical solution, in the satellite navigation denial scenario, the wireless positioning technology and the dead reckoning technology can be integrated, the problems of the traditional positioning technology in the application scenario of the ground-air cooperative unmanned system formation, such as inapplicability and unreliability, are solved, and the ground-air cooperative unmanned system formation can be supported in the cooperative positioning without relying on satellite positioning.

[0072] According to an embodiment of the present application, the kinematic state equation of the unmanned vehicle is as follows:

[0073]

[0074] wherein, are the lateral position coordinate, the longitudinal position coordinate and the heading angle of the unmanned vehicle at the i moment respectively, are the lateral position coordinate, the longitudinal position coordinate and the heading angle of the unmanned vehicle at the i-1 moment respectively, Δt is a sampling period, v i-1 is the linear velocity of the unmanned vehicle at the i-1 moment, ω i is the angular velocity of the unmanned vehicle at the i-1 moment.

[0075] According to an embodiment of the present application, the state transition equation of the unmanned vehicle is as follows:

[0076]

[0077] wherein, the superscript T is a transpose symbol, u i-1 is the real velocity of the unmanned vehicle at the i-1 moment, u m(i-1) is the measured velocity of the unmanned vehicle at the i-1 moment, w i-1 is the sensor noise of the Gaussian distribution at the i-1 moment.

[0078] According to an embodiment of the present application, the measurement equation of the master unmanned vehicle and the slave unmanned vehicle is as follows:

[0079]

[0080] wherein, Z i is the distance measurement value of the master unmanned vehicle and the slave unmanned vehicle, V i is the measurement noise, h(X i ) is an observation matrix, x ai , y ai are the lateral position coordinate and the longitudinal position coordinate of the master unmanned vehicle at the i moment respectively.

[0081] According to an embodiment of the present application, the linearized model is obtained, including:

[0082] f (·) is expanded around X i-1 the posterior value of X is expanded, and the Taylor series is truncated at the second order, to obtain

[0083]

[0084] h (·) is expanded around X i the prior value of X is expanded, and the Taylor series is truncated at the second order, to obtain

[0085]

[0086] where the first derivative H i of the above formula is

[0087]

[0088] According to an embodiment of the present application, the state prediction value and the update value are obtained by the following formula:

[0089]

[0090] wherein, is the state prediction value, P i / i-1 is the covariance prediction value, K i is the Kalman filtering coefficient, I is the unit matrix, is the state update value, P i is the covariance matrix, Q i-1 is the covariance matrix of the process noise at the i-1 moment.

[0091] According to an embodiment of the present application, the distance between the unmanned aerial vehicle and the master unmanned vehicle and the distance between the unmanned aerial vehicle and the slave unmanned vehicle are calculated by the following formula:

[0092]

[0093] A·X uav =B,

[0094]

[0095]

[0096] wherein, r1 is the distance between the unmanned aerial vehicle and the master unmanned vehicle, r2 is the distance between the unmanned aerial vehicle and the first slave unmanned vehicle, r3 is the distance between the unmanned aerial vehicle and the second slave unmanned vehicle, X uav =(x uav ,y uav ,z uav ) is the position coordinate of the unmanned aerial vehicle, X ugv1= (x1, y1, z1) is the position coordinate of the master unmanned vehicle, X ugv2 = (x2, y2, z2) is the position coordinate of the first slave unmanned vehicle, X ugv3 = (x3, y3, z3) is the position coordinate of the second slave unmanned vehicle.

[0097] According to an embodiment of the present application, the position coordinate of the unmanned aerial vehicle is obtained by the following formula:

[0098] X uav_i = (A i T ·A i ) -1 A i T ·B i ,

[0099] wherein X uav_i is the position coordinate of the unmanned aerial vehicle at time i.

[0100] According to an embodiment of the present application, the differential form of the inertial navigation system state equation is constructed based on an inertial navigation system error model:

[0101]

[0102] wherein F(t) is the system state transition matrix, X(t) is the system error state to be estimated, G(t) is the system noise transition matrix, and W(t) is the white noise error of the IMU.

[0103] According to an embodiment of the present application, the measurement equation based on the multi-source sensor is:

[0104] Z(t) = H · X(t) + V(t),

[0105] wherein H is the observation matrix, X(t) is the inertial navigation system state equation, and V(t) is the measurement noise.

[0106] The ground-air unmanned system cooperative positioning method described in the present application will be described below in conjunction with examples.

[0107] 1. Unmanned vehicle formation cooperative positioning

[0108] 1) Master-slave unmanned vehicle cooperative positioning system composition

[0109] As shown in Figure 1 , a master-slave unmanned vehicle formation can be formed by two unmanned vehicles, including a "master unmanned vehicle" carrying a high-precision IMU (inertial measurement unit), whose coordinate is defined as and a "slave unmanned vehicle" carrying a low-precision IMU, whose coordinate is defined as The unmanned vehicle is equipped with a wheel odometer and a UWB (ultra-wideband) wireless ranging sensor. Among them i , y i , are the horizontal and vertical position coordinates and the heading angle of the unmanned vehicle at time i, and the superscript T is the transpose symbol.

[0110] 2) Realize the dead reckoning positioning of the unmanned vehicle

[0111] For time i, define the linear velocity of the unmanned vehicle as v i , the angular velocity as ω i , and the sampling period as Δt. Then the kinematic state equation of the unmanned vehicle in the two-dimensional plane is as follows:

[0112]

[0113] Considering the influence of noise, define the true velocity u i =[v i ,ω i ] T , the measured velocity as u m(i) , and the sensor noise as w i , where u i =u m(i) -w i . P i is the state covariance matrix, indicating the uncertainty of the state estimation, and Q is the covariance matrix of the process noise.

[0114] Substitute the noise into equation (1) to obtain the state transition equation of the slave unmanned vehicle:

[0115]

[0116] where,

[0117]

[0118] 3) Fusion of wireless ranging and dead reckoning to realize cooperative positioning of the slave unmanned vehicle

[0119] Define the ranging value between the master-slave unmanned vehicles as Z i , and the measurement equation (measurement equation) between the master-slave unmanned vehicle formation is as follows:

[0120]

[0121] where V i is the measurement noise, R is the covariance of the measurement noise, and h(X i ) is the observation matrix.

[0122] According to the state equation and the measurement equation, the Kalman filter method (EKF) of the nonlinear system is used to linearize the system. At each time step, the approximation is realized by the first-order Taylor expansion of f(·) and h(·) around the current estimated point, and the linearized model is obtained.

[0123] Firstly, f(·) is expanded around the posterior value of X i-1 , and the Taylor series is truncated at the second order to obtain

[0124]

[0125] where the first derivative (Jacobian matrix) H i of the above equation can be solved as

[0126] Secondly, the nonlinear function h(·) is expanded around the prior value of X i , and the Taylor series is truncated at the second order to obtain

[0127]

[0128] where the first derivative (Jacobian matrix) H i of the above equation can be solved as

[0129]

[0130] Therefore, the state prediction and update of the master-slave unmanned vehicle formation based on the EKF recursive equation can be realized according to the following equation.

[0131]

[0132] 2. UAV cooperative positioning based on unmanned vehicle formation

[0133] 1) Composition of the ground-air cooperative positioning unmanned system

[0134] The ground-air cooperative positioning unmanned system is composed of a master-slave unmanned vehicle formation composed of three unmanned vehicles and a UAV, the master unmanned vehicle is installed with a high-precision IMU, the two slave unmanned vehicles are installed with medium-precision IMUs, the UAV is installed with a low-precision IMU, and each unmanned system is respectively loaded with a UWB (ultra-wideband) wireless ranging module, as shown in Figure 2 .

[0135] 2) UAV wireless positioning based on unmanned vehicle formation

[0136] Firstly, the distributed ranging equation is calculated as follows:

[0137]

[0138] Equation (9) can be simplified into the following linear equation vector form:

[0139] A·X uav = B (10)

[0140] wherein:

[0141]

[0142] Further, the least square method can be used to realize real-time wireless positioning of the unmanned aerial vehicle based on the unmanned vehicle formation position.

[0143] X uav_i = (A i T ·A i ) -1 ·A i T ·B i

[0144] 3) Fusion of wireless positioning and dead reckoning to realize cooperative positioning from unmanned aerial vehicle

[0145] The global observation vector is constructed for wireless positioning, magnetic compass and barometric altimeter, and then centralized processing is performed through Kalman filtering to obtain the global optimal estimation of the state vector. In order to construct the unmanned aerial vehicle motion equation based on inertial navigation, 15 order state variables are selected according to the unmanned aerial vehicle motion model:

[0146]

[0147] wherein, the navigation coordinate system is selected as the east-north-sky geographic coordinate system, φ E , φ N , φ U are east, north, and sky mathematical platform misalignment angles, δv E , δv N , δv U are east, north, and sky velocity errors, δL, δλ, δH are latitude, longitude, and height position errors, ε x , ε y , ε z are constant drifts of three gyroscopes in the carrier coordinate system, are constant biases of three accelerometers in the carrier coordinate system.

[0148] The differential form of the system state equation is constructed based on the error model of the inertial navigation system as follows:

[0149]

[0150] F(t) is the system state transition matrix, X(t) is the system error state to be estimated, G(t) is the system noise transition matrix, and W(t) is the white noise error of the IMU.

[0151] In addition, the magnetic compass and the barometer are used to correct the unmanned aerial vehicle heading error and height error. Define the wireless positioning plane coordinate information (the horizontal and vertical coordinates are represented as L, λ), the barometer height value (represented as H), the magnetic compass heading angle (represented as φ m ) and the residual error of the inertial navigation (INS) as the observation, and the global observation is:

[0152]

[0153] wherein φ INS is the inertial navigation heading angle, L INS is the inertial navigation horizontal coordinate, L wireless is the wireless positioning plane horizontal coordinate, λ INS is the inertial navigation vertical coordinate, λ wierless is the wireless positioning plane vertical coordinate, H INS is the inertial navigation height value, and H atm is the barometer height value.

[0154] The measurement equation of the system can be represented as:

[0155] Z(t) = H·X(t) + V(t) (16)

[0156] wherein H is the observation matrix, and V(t) is the measurement noise.

[0157] Finally, according to the state equation based on the error equation of the inertial navigation system and the measurement equation based on the multi-source sensor, the positioning result error of the unmanned aerial vehicle is corrected by the Kalman filter to improve the accuracy and stability of the navigation result.

[0158] For example, for a positioning system composed of 3 unmanned vehicles and 1 unmanned aerial vehicle, all the unmanned vehicles collect the wheel odometer information, the master unmanned vehicle is equipped with a high-precision IMU (Novatel PwrPak7D), and the two slave unmanned vehicles are equipped with a medium-precision IMU (Beidou Star Npos220). The unmanned aerial vehicle is equipped with a magnetic compass, a barometric altimeter and a low-precision IMU. All the unmanned platforms integrate a UWB distributed ranging unit and a differential GPS unit (RTK for short), wherein the high-precision differential positioning unit is only used to provide a positioning reference.

[0159] Firstly, the master-slave collaborative positioning based on the ground unmanned vehicle is realized, the master unmanned vehicle is equipped with a Novatel PwrPak7D high-precision inertial navigation, and the slave unmanned vehicle is equipped with a Beidou Star Npos220 medium-precision inertial navigation. The master unmanned vehicle performs dead reckoning positioning and synchronously shares its own position information to the slave unmanned vehicle. The slave unmanned vehicle implements collaborative positioning based on the wireless ranging information and its own dead reckoning result, thereby verifying the collaborative positioning capability of the ground unmanned vehicle formation.

[0160] The positioning accuracy comparison results are shown in Table 1 below, and it can be seen that after the unmanned vehicle is cooperatively positioned, the positioning accuracy is effectively improved.

[0161] Table 1 Comparison of ground unmanned vehicle formation cooperative positioning effect

[0162]

[0163] According to the experimental results, it can be seen that the wireless positioning of the unmanned aerial vehicle can effectively suppress the divergence problem of inertial navigation, and the dead reckoning method can effectively maintain the continuity of the unmanned aerial vehicle positioning trajectory in the scene where the wireless positioning fails.

[0164] The positioning error is recorded in Table 2 below, wherein the unmanned aerial vehicle relies on the real-time positioning accuracy of the unmanned vehicle formation node.

[0165] Table 2 Positioning error of ground-air cross-domain unmanned system formation

[0166]

[0167] The test results show that the master-slave ground-air unmanned system cooperative positioning method proposed in the application can guarantee and improve the positioning accuracy of the slave unmanned vehicle in the unmanned vehicle formation and the unmanned aerial vehicle.

[0168] As can be seen from the above embodiments, the cooperative positioning method described in the application combines the wireless ranging and dead reckoning ground-air unmanned system cooperative positioning strategy, first realizes the cooperative positioning of the unmanned vehicle formation, and then realizes the cooperative positioning of the unmanned aerial vehicle based on the unmanned vehicle formation. In the unmanned vehicle formation positioning, multiple unmanned vehicles carry different precision inertial sensors, take the dead reckoning and wireless ranging as the input information, adopt the fusion positioning method based on the extended Kalman filter, construct a master-slave positioning system, and improve the overall positioning accuracy of the unmanned vehicle formation. In the unmanned aerial vehicle positioning, a wireless multi-lateral positioning method based on the vehicle-mounted dynamic anchor point is adopted, and the information of the unmanned aerial vehicle dead reckoning and altimeter is fused to guarantee the positioning accuracy of the unmanned aerial vehicle.

[0169] In summary, the ground-air unmanned system cooperative positioning method described in the application improves the cooperative positioning accuracy and reliability of the ground-air unmanned system. Moreover, the application combines the distributed wireless positioning with the dead reckoning technology and integrates it into the distributed ground-air cross-domain cooperative unmanned system, improves the positioning accuracy of each unit node of the system through the filtering optimization algorithm, solves the unmanned system formation positioning problem in the satellite navigation failure scene, realizes the efficient cooperative positioning of the land-air unmanned system formation / cluster, and provides a reference for the cooperative positioning problem processing of other types of equipment facing the denial scene.

[0170] In the description of the application, it needs to be understood that the orientation words such as "front, back, upper, lower, left, right", "transverse, vertical, perpendicular, horizontal" and "top, bottom" and the like indicated orientation or position relationship is generally based on the orientation or position relationship shown in the drawings, only for the convenience of describing the application and simplifying the description, without making the opposite statement, these orientation words do not indicate and imply that the device or element referred to must have a particular orientation or be constructed and operated in a particular orientation, therefore, it cannot be understood as a limitation on the scope of protection of the application; the orientation words "inner, outer" refer to the inner and outer relative to the contour of each component.

[0171] For the convenience of description, spatial relative terms such as "over", "above", "upper surface", "upper" and the like can be used herein to describe the spatial position relationship of one device or feature with other devices or features as shown in the drawings. It should be understood that the spatial relative terms are intended to include different orientations in use or operation in addition to the orientation of the device described in the drawings. For example, if the device in the drawing is inverted, the device described as "above" or "over" other devices or structures will be positioned "below" or "under" other devices or structures. Thus, the exemplary term "above" can include both "above" and "below" orientations. The device can also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatial relative descriptions used herein are interpreted accordingly.

[0172] In addition, it should be noted that the use of "first", "second" and the like to define parts only facilitates the differentiation of corresponding parts, and the above words have no special meaning unless otherwise stated, and therefore cannot be understood as a limitation on the scope of protection of the application.

[0173] The above only describes the preferred embodiments of the application and is not intended to limit the application. For those skilled in the art, the application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the application shall be included in the scope of protection of the application.

Claims

1. A method for cooperative positioning of a ground-air unmanned system, characterized in that, The ground-air unmanned system comprises a master unmanned vehicle, a slave unmanned vehicle and a UAV, and the method comprises: determining a kinematic state equation of the slave unmanned vehicle; obtaining a state transition equation of the slave unmanned vehicle according to the kinematic state equation and Gaussian-distributed sensor noise; determining a measurement equation of the master unmanned vehicle and the slave unmanned vehicle; linearizing the system by using a Kalman filter method of a nonlinear system according to the state transition equation and the measurement equation to obtain a linearized model; performing state prediction and updating of the master-slave unmanned vehicle according to the linearized model to obtain state prediction values and updating values; determining a positioning result of the master unmanned vehicle and the slave unmanned vehicle according to the state prediction values and the updating values; calculating distances between the UAV and the master unmanned vehicle and distances between the UAV and the slave unmanned vehicle according to the positioning result; performing real-time wireless positioning of the UAV according to the calculated distances by using a least square method to obtain position coordinates of the UAV; determining a state variable according to a UAV motion model; constructing a differential form of an inertial navigation system state equation based on an inertial navigation system error model; determining a measurement equation based on multi-source sensors; performing error correction of the position coordinates of the UAV by using a Kalman filter according to the differential form of the inertial navigation system state equation and the measurement equation based on multi-source sensors to obtain corrected position coordinates; the kinematic state equation of the slave unmanned vehicle is: wherein, respectively, are the lateral position coordinate, the longitudinal position coordinate and the heading angle of the unmanned vehicle at time i, respectively, are the lateral position coordinate, the longitudinal position coordinate and the heading angle of the unmanned vehicle at time i-1, Δt is the sampling period, v i-1 is the linear velocity of the unmanned vehicle at time i-1; and i is the angular velocity of the unmanned vehicle at time i-1. the state transition equation of the slave unmanned vehicle is: wherein the superscript T is the transpose symbol, u i-1 is the real speed of the unmanned vehicle at time i-1, u m(i-1) is the measured speed of the unmanned vehicle at time i-1, w i-1 is the sensor noise of Gaussian distribution at time i-1; the measurement equation of the master unmanned vehicle and the slave unmanned vehicle is: wherein Z i is the distance value between the master unmanned vehicle and the slave unmanned vehicle, V i is the measurement noise, h(X i ) is the observation matrix, x ai , y ai are the horizontal position coordinate and the vertical position coordinate of the master unmanned vehicle at time i, respectively. the linearized model comprises: f(·) is centered around X i-1 the posterior value of X expanding, and dropping terms of order two and higher in the Taylor series, we get The nonlinear function h(·) is expanded around X i the prior value expanding, the Taylor series neglecting the second order and above terms, we get: where the first derivative H of the above formula is i H = 1 - x the state prediction values and the updating values are obtained by the following formula: wherein, is a state prediction value, P i / i-1 is a covariance prediction value, K i is a Kalman filter coefficient, I is an identity matrix, is a state update value, P i is a covariance matrix, Q i-1 is a covariance matrix of process noise at time i-1.

2. The method of claim 1, wherein, the distances between the UAV and the master unmanned vehicle and the distances between the UAV and the slave unmanned vehicle are calculated by the following formula: A•X uav = B, wherein r1 is the distance between the UAV and the master unmanned vehicle, r2 is the distance between the UAV and the first slave unmanned vehicle, r3 is the distance between the UAV and the second slave unmanned vehicle, X uav =(x uav ,y uav ,z uav ) is the position coordinate of the UAV, X ugv1 =(x1,y1,z1) is the position coordinate of the master unmanned vehicle, X ugv2 =(x2,y2,z2) is the position coordinate of the first slave unmanned vehicle, and X ugv3 =(x3,y3,z3) is the position coordinate of the second slave unmanned vehicle.

3. The method of claim 2, wherein, the position coordinates of the UAV are obtained by the following formula: X uav_i = (A i T ·A i ) -1 ·A i T ·B i , wherein X uav_i is the position coordinate of the UAV at time i.

4. The method of claim 3, wherein, the differential form of the inertial navigation system state equation is constructed based on the inertial navigation system error model: wherein F(t) is a system state transition matrix, X(t) is a system error state to be estimated, G(t) is a system noise transition matrix, and W(t) is white noise error of the IMU.

5. The method of claim 4, wherein, the measurement equation based on multi-source sensors is: Z(t) = H·X(t) + V(t), wherein H is an observation matrix, X(t) is the inertial navigation system state equation, and V(t) is measurement noise.

Images