Cooperative positioning method for ground-air unmanned system

Abstract

The invention discloses a cooperative positioning method for a ground-air unmanned system. The method comprises the following steps: determining a kinematics state equation of a slave unmanned vehicle; obtaining a state transition equation of the slave unmanned vehicle; determining a measurement equation of the master unmanned vehicle and the slave unmanned vehicle; a linearized model is obtained; state prediction and updating are carried out according to the linearized model; determining positioning results of the master unmanned vehicle and the slave unmanned vehicle; calculating 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 according to the positioning result; performing real-time wireless positioning on the unmanned aerial vehicle according to the calculated distance to obtain position coordinates of the unmanned aerial vehicle; determining a state variable according to the unmanned aerial vehicle motion model; constructing a differential form of an inertial navigation system state equation; determining a measurement equation based on a multi-source sensor; according to the inertial navigation system state equation in the differential form and the measurement equation based on the multi-source sensor, error correction is carried out on the position coordinates of the unmanned aerial vehicle through a Kalman filter, and the corrected position coordinates are obtained.

Description

Technical Field

[0001] The present invention relates to the field of positioning and navigation technology, and in particular to a collaborative positioning method for a ground-to-air unmanned system. Background Art

[0002] The ground-to-air unmanned system formation composed of unmanned vehicles and unmanned aerial vehicles can achieve complementarity in perception, navigation, load, communication, etc. through mutual cooperation, improve the flexibility of the entire system and its adaptability to unknown environments, and complete tasks that are difficult to complete by homogeneous unmanned vehicle formations or unmanned aerial vehicle formations, such as rescue search, regional inspection, geographic survey, map drawing, tracking and pursuit, etc.

[0003] Satellite navigation technology is an effective means to support the autonomous maneuvering of unmanned systems, but it is easily affected by complex factors such as space occlusion, electromagnetic interference, and deception, which restricts the application of ground-to-air collaborative unmanned systems; positioning methods based on laser point clouds and images are greatly affected by environmental images, and are prone to degradation or even failure in scenarios such as fast movement speed, unclear environmental geometry / texture features, many dynamic objects, or large changes in light / reflection levels; inertial technology, as a passive navigation technology, is less affected by external environmental factors, but there are cumulative errors after long-term operation; active wireless communication technologies represented by ultra-wideband technology can be used for local distance measurement, but the ranging error is large in dynamic scenarios and is not suitable for independent use. Summary of the invention

[0004] The present invention provides a ground-to-air unmanned system collaborative positioning method, which can solve the problems in the prior art.

[0005] The present invention provides a collaborative positioning method for a ground-to-air unmanned system, wherein the ground-to-air unmanned system includes a master unmanned vehicle, a slave unmanned vehicle and a drone, and the method includes:

[0006] Determine the kinematic state equations of the unmanned vehicle;

[0007] The state transfer equation of the unmanned vehicle is obtained based on the kinematic state equation and Gaussian distributed sensor noise;

[0008] Determine the measurement equations of the master unmanned vehicle and the slave unmanned vehicle;

[0009] According to the state transfer equation and the measurement equation, the Kalman filter method of nonlinear system is used to linearize the system and obtain a linearized model;

[0010] According to the linearized model, the state of the master-slave unmanned vehicle is predicted and updated to obtain the state prediction value and update value;

[0011] Determine the positioning results of the master unmanned vehicle and the slave unmanned vehicle according to the state prediction value and the update value;

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

[0013] The least square method is used to perform real-time wireless positioning of the UAV based on the calculated distance to obtain the position coordinates of the UAV;

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

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

[0016] Determine the measurement equation based on multi-source sensors;

[0017] According to the differential form of the inertial navigation system state equation and the measurement equation based on multi-source sensors, the Kalman filter is used to perform error correction on the position coordinates of the UAV to obtain the corrected position coordinates.

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

[0019]

[0020] in, are the horizontal position coordinate, vertical position coordinate and heading angle of the unmanned vehicle at time i, are the horizontal position coordinate, vertical position coordinate and heading angle of the unmanned vehicle at time i-1, Δt is the sampling period, v i-1 is the linear speed of the unmanned vehicle at time i-1, ω i is the angular velocity of the unmanned vehicle at time i-1.

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

[0022]

[0023] The superscript T is the transposition symbol, u i-1 is the actual 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.

[0024] Preferably, the measurement equations of the master unmanned vehicle and the slave unmanned vehicle are:

[0025]

[0026] Among them, Z i The distance measurement 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 They are the horizontal and vertical position coordinates of the main unmanned vehicle at time i respectively.

[0027] Preferably, obtaining a linearized model includes:

[0028] Revolve f(·) around X i-1 The posterior value of Expanding, the Taylor series ignores the terms above the second order, and we get:

[0029]

[0030] The nonlinear function h(·) is wrapped around X i The prior value of Expanding, the Taylor series ignores the terms above the second order, and we get:

[0031]

[0032] The first-order derivative H of the above formula i for:

[0033]

[0034] Preferably, the state prediction value and update value are obtained by the following formula:

[0035]

[0036] in, is the state prediction value, P i / i-1 is the covariance prediction value, K i is the Kalman filter 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 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 by the following formula:

[0038]

[0039] A.X uav =B,

[0040]

[0041] Among them, r1 is the distance between the UAV and the master UAV, r2 is the distance between the UAV and the first slave UAV, r3 is the distance between the UAV and the second slave UAV, and X uav =(x uav ,y uav ,Zuav ) is the position coordinate of the drone, X ugv1 =(x1, y1, z1) is the position coordinate of the main 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 coordinates of the drone are obtained by the following formula:

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

[0044] Among them, X uav_i is the position coordinate of the drone 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] Among them, F(t) is the system state transfer matrix, X(t) is the system error state to be estimated, G(t) is the system noise transfer matrix, and W(t) is the white noise error of IMU.

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

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

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

[0051] Through the above-mentioned technical solution, wireless positioning technology can be integrated with dead reckoning technology for satellite navigation denial scenarios, which solves the problems of unapplicability and unreliability of traditional positioning technology in the application scenarios of ground-to-air collaborative unmanned system formations, and can support the collaborative positioning of ground-to-air unmanned system formations without relying on satellite positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0053] Figure 1 A schematic diagram of a master-slave unmanned vehicle formation positioning scenario according to an embodiment of the present invention is shown;

[0054] Figure 2 A schematic diagram of a collaborative positioning scenario of a ground-to-air unmanned system formation according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0055] It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0056] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0057] Unless otherwise specifically stated, the relative arrangement of the parts and steps described in these embodiments, numerical expressions and numerical values ​​do not limit the scope of the present invention. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be regarded as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0058] An embodiment of the present invention provides a ground-to-air unmanned system collaborative positioning method, wherein the ground-to-air unmanned system includes a master unmanned vehicle, a slave unmanned vehicle and a drone, and the method includes:

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

[0060] The state transfer equation of the unmanned vehicle is obtained based on the kinematic state equation and Gaussian distributed sensor noise;

[0061] Determine the measurement equations of the master unmanned vehicle and the slave unmanned vehicle;

[0062] According to the state transfer equation and the measurement equation, the Kalman filter method of nonlinear system is used to linearize the system and obtain a linearized model;

[0063] According to the linearized model, the state of the master-slave unmanned vehicle is predicted and updated to obtain the state prediction value and update value;

[0064] Determine the positioning results of the master unmanned vehicle and the slave unmanned vehicle according to the state prediction value and the update value;

[0065] According to the positioning results, the distance between the UAV and the master unmanned vehicle and the distance between the UAV and the slave unmanned vehicle are calculated;

[0066] The least square method is used to perform real-time wireless positioning of the UAV based on the calculated distance to obtain the position coordinates of the UAV;

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

[0068] Construct the differential form of the inertial navigation system state equation based on the inertial navigation system error model;

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

[0070] According to the differential form of the inertial navigation system state equation and the measurement equation based on multi-source sensors, the Kalman filter is used to perform error correction on the position coordinates of the UAV to obtain the corrected position coordinates.

[0071] Through the above-mentioned technical solution, wireless positioning technology can be integrated with dead reckoning technology for satellite navigation denial scenarios, which solves the problems of unapplicability and unreliability of traditional positioning technology in the application scenarios of ground-to-air collaborative unmanned system formations, and can support the collaborative positioning of ground-to-air unmanned system formations without relying on satellite positioning.

[0072] According to one embodiment of the present invention, from the kinematic state equation of the unmanned vehicle:

[0073]

[0074] in, are the horizontal position coordinate, vertical position coordinate and heading angle of the unmanned vehicle at time i, are the horizontal position coordinate, vertical position coordinate and heading angle of the unmanned vehicle at time i-1, Δt is the sampling period, v i-1 is the linear speed of the unmanned vehicle at time i-1, ω i is the angular velocity of the unmanned vehicle at time i-1.

[0075] According to one embodiment of the present invention, the state transfer equation of the unmanned vehicle is:

[0076]

[0077] The superscript T is the transposition symbol, u i-1 is the actual 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.

[0078] According to one embodiment of the present invention, the measurement equations of the master unmanned vehicle and the slave unmanned vehicle are:

[0079]

[0080] Among them, Z i The distance measurement 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 They are the horizontal and vertical position coordinates of the main unmanned vehicle at time i respectively.

[0081] According to one embodiment of the present invention, obtaining a linearized model includes:

[0082] Revolve f(·) around X i-1 The posterior value of Expanding, the Taylor series ignores the terms above the second order, and we get:

[0083]

[0084] The nonlinear function h(·) is wrapped around X i The prior value of Expanding, the Taylor series ignores the terms above the second order, and we get:

[0085]

[0086] The first-order derivative H of the above formula i for:

[0087]

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

[0089]

[0090] in, is the state prediction value, P i / i-1 is the covariance prediction value, K i is the Kalman filter 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 time i-1.

[0091] According to one embodiment of the present invention, the distance between the drone and the master unmanned vehicle and the distance between the drone and the slave unmanned vehicle are calculated by the following formula:

[0092]

[0093] A.X uav =B,

[0094]

[0095]

[0096] Among them, r1 is the distance between the UAV and the master UAV, r2 is the distance between the UAV and the first slave UAV, r3 is the distance between the UAV and the second slave UAV, and X uav =(x uav ,y uav ,z uav ) is the position coordinate of the drone, X ugv1=(x1, y1, z1) is the position coordinate of the main 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 one embodiment of the present invention, the position coordinates of the drone are obtained by the following formula:

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

[0099] Among them, X uav_i is the position coordinate of the drone at time i.

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

[0101]

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

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

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

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

[0106] The ground-to-air unmanned system collaborative positioning method of the present invention is described below with reference to examples.

[0107] 1. Unmanned vehicle platooning collaborative positioning

[0108] 1) Composition of the master-slave unmanned vehicle collaborative positioning system

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

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

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

[0112]

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

[0114] Substituting the noise effect into equation (1), we get the state transfer equation of the unmanned vehicle:

[0115]

[0116] in,

[0117]

[0118] 3) Integrate wireless ranging and dead reckoning to achieve collaborative positioning from unmanned vehicles

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

[0120]

[0121] Where V i is the measurement noise, whose covariance is R, h(X i )Observation matrix.

[0122] According to the state equation and measurement equation, the system is linearized by using the Kalman filter method (EKF) of nonlinear systems. In each time step, the linearized model is obtained by performing a first-order Taylor expansion of f(·) and h(·) near the current estimation point.

[0123] First, we transform f(·) around X i-1 The posterior value of Expanding, the Taylor series ignores the terms above the second order, and we get

[0124]

[0125] in

[0126] Second, we apply the nonlinear function h(·) around X i The prior value of Expanding, the Taylor series ignores the terms above the second order, and we get

[0127]

[0128] The first-order derivative (Jacobian matrix) of the above formula is H i can be solved as

[0129]

[0130] Therefore, the master-slave unmanned vehicle formation can realize state prediction and update based on the EKF recursive equation as follows.

[0131]

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

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

[0134] The ground-air collaborative positioning unmanned system consists of a master-slave unmanned vehicle formation consisting of three unmanned vehicles and a drone. The master unmanned vehicle is equipped with a high-precision IMU, the two slave unmanned vehicles are equipped with a medium-precision IMU, and the drone is equipped with a low-precision IMU. Each unmanned system is equipped with a UWB (ultra-wideband) wireless ranging module, see Figure 2 .

[0135] 2) Wireless positioning of drones based on unmanned vehicle formations

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

[0137]

[0138] Equation (9) can be simplified to the following linear equations in vector form:

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

[0140] in:

[0141]

[0142] Furthermore, the least squares method can be used to achieve real-time wireless positioning of the UAV based on the position of the unmanned vehicle formation.

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

[0144] 3) Integrate wireless positioning and dead reckoning to achieve collaborative positioning from drones

[0145] The global observation vector is constructed for wireless positioning, magnetic compass, and barometric altimeter, and then processed centrally through Kalman filtering to obtain the global optimal estimate of the state vector. In order to construct the UAV motion equation based on inertial navigation, the 15th-order state variable is selected according to the UAV motion model:

[0146]

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

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

[0149]

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

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

[0152]

[0153] Among them, φ INS is the inertial navigation heading angle, L INS is the inertial navigation abscissa, L wireless is the horizontal coordinate of the wireless positioning plane, λ INS is the inertial navigation ordinate, λ wierless is the vertical coordinate of the wireless positioning plane, H INS Inertial navigation altitude value, H atm is the barometer altitude value.

[0154] Then the measurement equation of the system can be expressed as:

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

[0156] Where H is the measurement matrix and V(t) is the measurement noise.

[0157] Finally, according to the state equation based on the inertial navigation system error equation and the measurement equation based on multi-source sensors, the Kalman filter is used to correct the positioning result error of the UAV to improve the accuracy and stability of the navigation results.

[0158] For example, for a positioning system consisting of three unmanned vehicles and one drone, all unmanned vehicles collect wheel odometer information, the main unmanned vehicle is equipped with a high-precision IMU (Novatech PwrPak7D), and the two slave unmanned vehicles are equipped with medium-precision IMUs (BDStar Npos220). The drone is equipped with a magnetic compass, a barometric altimeter, and a low-precision IMU. All unmanned platforms integrate UWB distributed ranging units and differential GPS units (RTK for short), of which the high-precision differential positioning unit is only used to provide a positioning reference.

[0159] First, master-slave collaborative positioning based on ground unmanned vehicles is realized. The master unmanned vehicle is equipped with Novatel PwrPak7D high-precision inertial navigation, and the slave unmanned vehicle is equipped with Beidou Navigation's Npos220 medium-precision inertial navigation. The master unmanned vehicle is positioned through dead reckoning and synchronously shares its own position information with the slave unmanned vehicle. The slave unmanned vehicle uses this position information to implement collaborative positioning based on wireless ranging information and its own dead reckoning results, thereby verifying the collaborative positioning capability of the ground unmanned vehicle formation.

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

[0161] Table 1 Comparison of collaborative positioning effects of ground unmanned vehicle formations

[0162]

[0163] According to the experimental results, UAV wireless positioning can effectively suppress the inertial navigation divergence problem, and the dead reckoning method can effectively maintain the continuity of the UAV positioning trajectory in the scenario of wireless positioning failure.

[0164] The positioning error records are shown in Table 2 below, where the UAV relies on the real-time positioning accuracy of the unmanned vehicle formation node.

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

[0166]

[0167] The test results show that the master-slave ground-to-air unmanned system collaborative positioning method proposed in the present invention can ensure and improve the positioning accuracy of slave unmanned vehicles and unmanned aerial vehicles in an unmanned vehicle formation.

[0168] It can be seen from the above embodiments that the collaborative positioning method described in the present invention integrates the ground-to-air unmanned system collaborative positioning strategy of wireless ranging and dead reckoning, firstly realizing the collaborative positioning of unmanned vehicle formations, and secondly realizing the collaborative positioning of drones based on the unmanned vehicle formations. In the positioning of unmanned vehicle formations, multiple unmanned vehicles are equipped with inertial sensors of different precisions, using dead reckoning and wireless ranging as input information, and adopting a fusion positioning method based on extended Kalman filtering to construct a master-slave positioning system, thereby improving the overall positioning accuracy of the unmanned vehicle formation. In the positioning of drones, a wireless multilateral positioning method based on vehicle-mounted dynamic anchor points is adopted, and information such as drone dead reckoning and altimeter are integrated to ensure the positioning accuracy of drones.

[0169] In summary, the ground-to-air unmanned system collaborative positioning method described in the present invention improves the accuracy and reliability of the collaborative positioning of the ground-to-air unmanned system. In addition, the present invention integrates distributed wireless positioning with dead reckoning technology, and integrates and applies it to the distributed ground-to-air cross-domain collaborative unmanned system. The positioning accuracy of each unit node of the system is improved through the filtering optimization algorithm, which solves the problem of unmanned system formation positioning in the scenario of satellite navigation failure, realizes efficient collaborative positioning of land-to-air unmanned system formation / cluster, and provides reference for the collaborative positioning problem processing of other types of equipment facing denial scenarios.

[0170] In the description of the present invention, it is necessary to understand that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "lateral, vertical, perpendicular, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description. Unless otherwise specified, these directional words do not indicate or imply that the devices or elements referred to must have a specific direction or be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the scope of protection of the present invention. The directional words "inside and outside" refer to the inside and outside relative to the contours of each component itself.

[0171] For ease of description, spatially relative terms such as "above", "above", "on the upper surface of", "above", etc. may be used here to describe the spatial positional relationship between a device or feature and other devices or features as shown in the figure. It should be understood that spatially relative terms are intended to include different orientations of the device in use or operation in addition to the orientation described in the figure. For example, if the device in the accompanying drawings is inverted, the device described as "above other devices or structures" or "above other devices or structures" will be positioned as "below other devices or structures" or "below other devices or structures". Thus, the exemplary term "above" can include both "above" and "below". The device can also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatially relative descriptions used here are interpreted accordingly.

[0172] In addition, it should be noted that the use of terms such as "first" and "second" to limit components is only for the convenience of distinguishing the corresponding components. If not otherwise stated, the above terms have no special meaning and therefore cannot be understood as limiting the scope of protection of the present invention.

[0173] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for collaborative positioning of ground-to-air unmanned systems, characterized in that: The ground-to-air unmanned system includes a master unmanned vehicle, a slave unmanned vehicle and a drone, and the method includes: Determine the kinematic state equations of the unmanned vehicle; The state transfer equation of the unmanned vehicle is obtained based on the kinematic state equation and Gaussian distributed sensor noise; Determine the measurement equations of the master unmanned vehicle and the slave unmanned vehicle; According to the state transfer equation and the measurement equation, the Kalman filter method of nonlinear system is used to linearize the system and obtain a linearized model; According to the linearized model, the state of the master-slave unmanned vehicle is predicted and updated to obtain the state prediction value and update value; Determine the positioning results of the master unmanned vehicle and the slave unmanned vehicle according to the state prediction value and the update value; According to the positioning results, the distance between the UAV and the master UAV and the distance between the UAV and the slave UAV are calculated; The least square method is used to perform real-time wireless positioning of the UAV based on the calculated distance to obtain the position coordinates of the UAV; Determine state variables based on the UAV motion model; Construct the differential form of the inertial navigation system state equation based on the inertial navigation system error model; Determine the measurement equation based on multi-source sensors; According to the differential form of the inertial navigation system state equation and the measurement equation based on multi-source sensors, the Kalman filter is used to perform error correction on the position coordinates of the UAV to obtain the corrected position coordinates.

2. The method according to claim 1, characterized in that From the kinematic state equation of the unmanned vehicle: in, are the horizontal position coordinate, vertical position coordinate and heading angle of the unmanned vehicle at time i, are the horizontal position coordinate, vertical position coordinate and heading angle of the unmanned vehicle at time i-1, Δt is the sampling period, v i-1 is the linear speed of the unmanned vehicle at time i-1, ω i is the angular velocity of the unmanned vehicle at time i-1.

3. The method according to claim 2, characterized in that The state transfer equation of the unmanned vehicle is: The superscript T is the transposition 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.

4. The method according to claim 3, characterized in that: The measurement equations of the master unmanned vehicle and the slave unmanned vehicle are: Among them, Z i The distance measurement 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 They are the horizontal and vertical position coordinates of the main unmanned vehicle at time i respectively.

5. The method according to claim 4, characterized in that The linearized models include: Revolve f(·) around X i-1 The posterior value of Expanding, the Taylor series ignores the terms above the second order, and we get: The nonlinear function h(·) is wrapped around X i The prior value of Expanding, the Taylor series ignores the terms above the second order, and we get: The first-order derivative H i for:

6. The method according to claim 5, characterized in that The state prediction value and update value are obtained by the following formula: in, is the state prediction value, P i / i-1 is the covariance prediction value, K i is the Kalman filter 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 time i-1.

7. The method according to claim 6, characterized in that The distance between the UAV and the master UAV and the distance between the UAV and the slave UAV are calculated by the following formula: A·X uav =B, Among them, r1 is the distance between the UAV and the master UAV, r2 is the distance between the UAV and the first slave UAV, r3 is the distance between the UAV and the second slave UAV, and X uav =(x uav ,y uav ,z uav ) is the position coordinate of the drone, X ugv1 =(x1, y1, z1) is the position coordinate of the main unmanned vehicle, X ugv2 =(x2,y2,z2) is the position coordinate of the first slave unmanned vehicle, X ugv3 =(x3,y3,x3) is the position coordinate of the second slave unmanned vehicle.

8. The method according to claim 7, characterized in that The position coordinates of the drone are obtained by the following formula: X uav_i =(A i T ·A i ) -1 ·A i T ·B i , Among them, X uav_i is the position coordinate of the drone at time i.

9. The method according to claim 8, characterized in that The differential form of the inertial navigation system state equation is constructed based on the inertial navigation system error model: Among them, F(t) is the system state transfer matrix, X(t) is the system error state to be estimated, G(t) is the system noise transfer matrix, and W(t) is the white noise error of IMU.

10. The method according to claim 8, characterized in that The measurement equation based on multi-source sensors is: Z(t)=H·X(t)+V(t), Where H is the observation matrix, X(t) is the state equation of the inertial navigation system, and V(t) is the measurement noise.

Images