CN116793356A - Method and system for estimating relative pose between mobile devices by combining IMU and data chain - Google Patents

Abstract

The invention provides a method and a system for estimating relative pose between mobile equipment combined by an IMU and a data chain, wherein a rigid IMU sensor and the data chain equipment are arranged on the mobile equipment, after calibration is completed, the IMU equipment uses an inertial navigation calculation method to obtain the position and pose information of the mobile equipment under a navigation coordinate system, and the position and pose information is integrated into a state information frame to be output through the data chain; calculating the distance between two mobile devices through the time difference of data link information release and receiving; using the azimuth angle and the altitude angle of the data link equipment when receiving information, calculating the position of the information sending equipment under the northeast day coordinate system of the receiving equipment; establishing a relative position residual error by using the calculated relative position of the transmitting equipment under the receiving equipment body coordinate system and the relative position of the transmitting equipment under the receiving equipment body coordinate system, which is obtained by measuring the data chain; and then based on the time sequence, establishing a relative pose optimization equation to optimize and calculate the relative pose among the positioning devices.

Description

Method and system for estimating relative pose between mobile devices by combining IMU and data chain

Technical Field

The invention relates to the fields of autonomous positioning and equipment relative positioning of mobile equipment, in particular to a method and a system for estimating relative pose between mobile equipment combined by an IMU and a data chain.

Background

In recent years, mobile device clusters have become a major direction of mobile robot technology development. Accurate positioning information is an important guarantee for reliably executing tasks by the clusters, and accurate estimation of relative pose information of mobile equipment in the clusters in real time is a key technology for cluster positioning.

Schemes for estimating relative pose among clusters of mobile devices can be divided into two schemes based on external sensors and based on own sensors. The scheme based on the external sensor mainly depends on GNSS positioning equipment for providing absolute longitude and latitude information, can realize centimeter-level precision, and is adopted by the current outdoor large-scale unmanned aerial vehicle cluster equipment. In the global satellite navigation system (Global Navigation Satellite System, GNSS) refusing environment, an optical capturing system or an Ultra Wide Band (UWB) based local positioning system is often adopted to realize the relative positioning between devices in the mobile device cluster, but these schemes are only suitable for indoor use or small use, which severely limits the task capability of the mobile device.

With the increasing complexity of task scenes of mobile equipment, a relative positioning scheme based on own sensors becomes a hot spot of relative positioning research among current clusters due to the characteristics of flexibility and easiness in implementation. The existing relative positioning algorithm mainly depends on an object recognition method, for example, a four-rotor unmanned aerial vehicle relative pose estimation method based on luminous body ring detection, which is proposed by Fei, is realized by recognizing luminous rings on unmanned aerial vehicles, and the relative pose between unmanned aerial vehicles is calculated, so that the method is only suitable for small-scale unmanned equipment clusters. Xie Nian an unmanned aerial vehicle formation relative pose estimation method based on monocular visual information is provided, markers with different geometric shapes are installed for each unmanned aerial vehicle, estimation of relative poses among unmanned aerial vehicles is achieved through recognition of the markers, and the method needs to be used for modifying mobile equipment hardware and can only be applied to task scenes with relatively close distances among unmanned aerial vehicles in a cluster.

Disclosure of Invention

In order to solve the problems in the prior art, the invention provides a relative pose estimation method and a relative pose estimation system between mobile devices based on an inertial motion unit (Inertial Motion Unit, IMU) and a data chain, wherein the IMU and the data chain are combined, the IMU and the data chain are rigidly connected to a positioning device, and the relative pose between the mobile devices with high precision is obtained by measuring the motion of the positioning device and the relative position between the positioning devices.

The technical scheme of the invention is as follows:

the method for estimating the relative pose between the mobile equipment combined by the IMU and the data chain comprises the steps that positioning equipment is arranged on the mobile equipment and consists of the IMU equipment and the data chain equipment which are rigidly connected; in the relative pose estimation process, the positioning devices can communicate in real time, and the data acquired by respective IMU and the self-position and pose information obtained by respective calculation are interacted;

the relative pose estimation method comprises the following steps:

step 1: for each mobile device, acquiring the values of acceleration and angular velocity in real time through an IMU device installed on the mobile device, and integrating the acceleration and the angular velocity by using an inertial navigation calculation method to obtain the position and the attitude information of the mobile device under a navigation coordinate system;

step 2: integrating the calculated position, speed and pose information of the mobile equipment into a state information frame of the mobile equipment at each moment, and outputting the state information frame through a data chain;

step 3: establishing a data link distance calculation model, and calculating the distance between two mobile devices through the time difference of data link information release and receiving; using the azimuth angle and the altitude angle of the data link equipment when receiving information, calculating the position of the information sending equipment under the northeast day coordinate system of the receiving equipment;

Step 4: based on the step 1, the positioning equipment obtains the pose information of the positioning equipment through IMU-based calculation, and the positioning equipment transmits the pose information obtained through IMU-based calculation to other positioning equipment through a data link; after receiving the information, the positioning equipment at the receiving end analyzes the information and obtains a relative distance measurement value between the two mobile equipment based on data link measurement by the method of the step 3;

and at the receiving end, the pose information and the data chain measurement information of the receiving end are aligned in time by adopting a linear difference value based on the data chain measurement time:

constructing a data structure comprising the measuring moment of the data chain, the relative distance measured by the data chain and the measured relative angle: a distance frame; constructing a data structure containing the moment of inertial navigation pose calculation and the position and the pose of inertial navigation calculation: inertial navigation pose frames;

after receiving the data link information sent by the mobile device b, the data link receiving end of the mobile device a calculates the relative distance and angle measured by the data link, constructs the relative distance and angle into a distance frame, and stores the distance frame into a queue of the distance frame; meanwhile, inertial navigation information of the mobile equipment b is contained in the data chain information, an inertial navigation pose frame is constructed, and the inertial navigation pose frame is stored in an inertial navigation pose frame queue;

Taking the received time stamp of the second distance frame as the initial time stamp t of the system _D0 Taking the timestamp mean value of two adjacent inertial navigation pose frames before and after the second distance frame as the initial moment of the inertial navigation pose frame of the transmitting end:

the system time stamp of the following kth inertial navigation pose frame and the nth distance frame is as follows:

t is in _Ik Acquisition time stamp, t for kth inertial navigation pose frame _Dn An acquisition timestamp for the nth distance frame;

searching two consecutive inertial navigation pose frames meeting the conditions in the stored inertial navigation pose frame queue:

in the middle ofAnd respectively carrying out linear difference on the inertial pose frames of the kth frame and the k+1 frame for the time stamps of the previous inertial pose frame and the next inertial pose frame when the nth distance frame is acquired, wherein the formula is as follows:

obtaining P ⁿ ，θ ⁿ Estimating a position and a posture of the mobile device for inertial navigation after time alignment; p (P) ^k ，P ^k+1 And theta ^k ，θ ^k+1 The position and the gesture of the mobile device estimated in inertial navigation frames of the kth frame and the k+1th frame respectively;

step 5: after alignment of positioning information data is completed, pose information of positioning equipment for transmitting information is arranged, the pose of the positioning equipment is converted into a machine body coordinate system of receiving information equipment, and a relative position residual error is established by using the calculated relative position of the transmitting equipment under the machine body coordinate system of the receiving equipment and the relative position of the transmitting equipment under the machine body coordinate system of the receiving equipment, which is obtained by measuring a data chain;

Step 6: based on the time sequence, the relative position residuals at the first m moments are combined, a relative pose optimization equation is established, and the relative pose among positioning devices is optimized and calculated by adopting a Levenberg-Ma Kaer special method.

Further, calibrating the relative pose between the IMU and the data chain: the IMU coordinate system is used as a positioning equipment coordinate system, and the relative pose transformation of the data chain coordinate system and the IMU coordinate system is obtained through calibration by a manual measurement method

Further, the data link measurement delay is calibrated:

the calculation formula of the measurement distance between a pair of data chains is as follows:

in the middle ofWhen a signal is sent to the data link a on one mobile device, after the data link b on the other mobile device receives the signal sent by the data link a, the signal is reverted to the data link a signal, and the time when the data link a receives the reverted signal is +.>c is the speed of light, δt is the delay of the receiving, transmitting and processing time of the data chain;

and solving to obtain δt through the n ranging results and GNSS information at corresponding moments, wherein the calculation formula is as follows:

d ^e ＝||T _el P ^a -T _el P ^b ||

d in ^m ，d ^e The measured value and the calculated value of the distance are respectively,and->Respectively measuring and calculating the distance at the moment i; t (T) _el Is a transformation matrix from a longitude and latitude high coordinate system to a geocentric ground rectangular coordinate system, P ^a ，P ^b The positioning device is used for sending the data link information and receiving the position information of the positioning device of the data link information under a longitude and latitude high coordinate system.

Further, in step 1, the inertial navigation calculation method is as follows:

step 1.1: and obtaining an attitude transformation matrix from the body coordinate system of the k-moment mobile equipment to the navigation coordinate system by using the attitude angle measured by the k-moment gyroscope through the following processes:

firstly, solving a gesture transformation matrix from a body coordinate system at the current moment to a navigation coordinate system, and obtaining the pose of the mobile equipment at the current moment, wherein the calculation method comprises the following steps:

wherein:is a gesture transformation matrix from a machine body coordinate system to a navigation coordinate system at the moment k, ⁿ T _k,k-1 for navigating the transformation matrix of the coordinate system from time k-1 to time k, the transformation matrix is a unit matrix；/>The transformation matrix from the machine body coordinate system to the navigation coordinate system at the moment k-1; />The angular velocity integral solution is obtained by measuring a gyroscope from the moment k-1 to the moment k for the pose change matrix of the machine body coordinate system from the moment k-1 to the moment k; omega (τ) is the angular velocity measured by the gyroscope at τ, Δθ _k For the IMU pose rotation quantity from k-1 to k time _k The attitude quantity of the IMU at the moment k under the navigation coordinate system;

step 1.2: the acceleration a measured by an accelerometer on a machine body coordinate system at the moment k ^b Transformation matrix from machine body coordinate system at k moment to navigation coordinate systemConversion to a navigation coordinate system:

step 1.3: using the acceleration value in the navigation coordinate system obtained in the step 1.2 and using a velocity differential equation

Calculating a mobile device speed, whereIs the differential value of inertial navigation speed, f ⁿ For the acceleration measured on the navigation coordinate system, < >>Is a Ke type acceleration, wherein V ⁿ For the speed of the mobile device in the navigational coordinate system, < >>The rotation angular velocity of the earth in the navigation coordinate system; />Centripetal acceleration induced for carrier movement, wherein +.>The rotation angular velocity of the mobile device around the earth in a navigation coordinate system; g ⁿ Is the gravitational acceleration under a navigation coordinate system; sampling the increment information in each updating period, and then the speed of the mobile device at the moment k is as follows:

in the middle ofFor k, k-1 the mobile device speeds in the navigation coordinate system, +.>For the pose transformation matrix from the machine system at the time of k to the navigation coordinate system,/for the navigation coordinate system>The pose transformation matrix is a pose transformation matrix of the machine body coordinate system from the moment t to the moment k;

step 1.4: and (3) utilizing the speed of the mobile equipment in the navigation coordinate system obtained in the step (1.3), and adopting a trapezoidal integration method to realize an update equation of the position of the mobile equipment:

Wherein P is _k-1 And P _k The positions of the mobile equipment at the time k-1 and the time k under a navigation coordinate system respectively comprise latitude L, longitude lambda and altitude h of the mobile equipment; τ is the time interval between time k-1 and time k; m is M _pv(k-1/2) The matrix is a transformation matrix for converting the navigation coordinates at the moment of k-1/2 into longitude and latitude high coordinates; r is R _M And R is _N The main curvature radiuses of the meridian circle and the mortise circle of the position of the mobile device are respectively represented.

Further, in step 3, bad distance measurements are screened out by:

the dead pixel is removed by calculating the relative distance measured value at the continuous time and comparing the distance measured value variation between the adjacent time:

δd _k-1 ＝d _k-1 -d _k-2

δd _k ＝d _k -d _k-1

d in _k-2 ,d _k-1 ,d _k Relative distance measurements at the k-2, k-1, k times, respectively; δd _k-1 Is the difference between the relative distance measurements at time k-2 and time k-1, δd _k For the difference between the relative distance measurement values at the kth time and the kth-1 time, Δd is the relative distance change threshold, and the continuous rest time δd should be taken into account _k-1 Case=0.

Further, in step 5, the relative position residual error is established after integrating the coordinate system, and the specific process is as follows:

step 5.1: storing the latest m distance frames received and the inertial pose frames obtained by corresponding interpolation;

Step 5.2: establishing a position residual error:

in the middle ofMeasuring a data chain to obtain the position of the transmitting positioning equipment b corresponding to the k-i frame state parameter under the body coordinate system of the receiving positioning equipment a; />The position of the transmitting positioning equipment b under the body coordinate system of the receiving positioning equipment a is estimated for inertial navigation;

the data link measures the position of the transmitting positioning device b under the northeast and north day coordinate system of the k-i moment of the receiving positioning device a, and converts the position into the position of the transmitting positioning device b under the body coordinate system of the receiving positioning device a, and the calculation formula is as follows:

in the middle ofTo receive the pose transformation matrix of the positioning device a from the navigation coordinate system to the machine body coordinate system at the moment k-i,transmitting the position of the positioning device b under the navigation coordinate system of the positioning device a for the moment k-i;

the calculation formula of the position of the inertial navigation estimated transmitting positioning device b under the body coordinate system of the positioning device a is as follows:

in the middle ofFor receiving the transformation matrix of the k-moment coordinate system of the positioning device a into the k-i-moment coordinate system,/>For transmitting the change matrix of the k-i time coordinate system of positioning device b to the k time coordinate system,/->For the relative pose of transmitting positioning device b at time k under the coordinate system of receiving positioning device a,/->Transmitting the position of the positioning device b under the coordinate system of the transmitting positioning device b for the moment k-i;

The pose of the mobile equipment obtained by using inertial navigation estimation is the relative pose under the navigation coordinate system, then Is calculated as follows:

in the middle ofTransmitting the northeast coordinate system of positioning device b to the body seat for time kThe pose transformation matrix of the standard system;fixing the pose transformation matrix from the rectangular coordinate system to the northeast and north day coordinate system of the transmitting and positioning equipment b for the earth center at the moment k; />Transmitting a pose transformation matrix from a north-east coordinate system of the positioning equipment b to a geocentric fixed coordinate system for the moment k-i; />And transmitting the machine system of the positioning equipment b to a pose transformation matrix under the north-east coordinate system of the positioning equipment b at the moment k-i.

Further, the specific process of step 6 is as follows:

and (3) carrying out weighted summation on the relative position residuals established at the first m times of the current kth time, and establishing a relative pose estimation optimization equation f (x) based on a time sequence, wherein the relative pose estimation optimization equation f (x) is as follows:

w in the formula ^k-i The weight of the k-i th relative position error,the position residual error established in the step 5 is obtained; the optimized variable x is the relative pose of the body coordinate system of the positioning equipment b at the current moment under the body coordinate system of the positioning equipment a.

An inter-mobile device relative positioning system of an IMU in combination with a data chain, comprising: the system comprises an inertial navigation module, a data link measurement module, a flight state parameter, a data link measurement time alignment module and a relative pose estimation module; wherein:

The inertial navigation module is used for solving the flight pose according to the acceleration and the angular velocity acquired by the accelerometer and the gyroscope, integrating the flight pose into a state parameter frame of the positioning equipment, and transmitting the obtained state parameter frame at each moment to other mobile equipment through a data chain;

and the data chain measuring module is used for calculating the distance between the positioning devices according to the receiving and releasing time of the data chain information frames and removing the erroneous measured value by adopting a motion model. According to the angle and the distance value when the data are linked, decomposing the distance to obtain the relative position of the positioning equipment b under the north-east coordinate system of the positioning equipment a;

the flight state parameter and data link measurement time alignment module is used for carrying out linear interpolation on the pose estimated by the inertial navigation of the positioning equipment b based on the data link time stamp to obtain the flight state parameter at the measurement moment;

the relative pose estimation module converts the relative position measured by the data chain into a machine body coordinate system, and calculates the relative position between the mobile devices at each moment according to the first m state parameters; using the relative position between the mobile devices obtained by measuring the data chain and the relative position obtained by calculating the inertial navigation to construct a residual error of the relative position between the airplanes, and establishing a relative pose optimization equation between the mobile devices based on a time sequence; and (5) performing iterative optimization by using a Levenberg-Ma Kaer special method to obtain the real-time relative pose between the mobile devices.

Advantageous effects

Compared with the prior art, the invention has the advantages that:

the method can realize high-precision relative pose estimation between mobile devices only by relying on inertial navigation equipment and a data chain capable of measuring distance and angle.

The invention can estimate the relative pose between the remote mobile devices without changing the structure of the mobile devices and using the mobile device identification algorithm based on the shape characteristics.

The invention adopts the method of combining and optimizing the inertial navigation positioning information and the data link measurement information, and can realize the autonomous positioning of the mobile equipment and the estimation of the relative pose between the mobile equipment under the GNSS refusing environment.

Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.

Drawings

The foregoing and/or additional aspects and advantages of the invention will become apparent and may be better understood from the following description of embodiments taken in conjunction with the accompanying drawings in which:

fig. 1 is a flow chart of a method of relative positioning between mobile devices with a data chain and IMU combination.

Detailed Description

The following detailed description of embodiments of the invention is exemplary and intended to be illustrative of the invention and not to be construed as limiting the invention.

As shown in fig. 1, in this embodiment, two airplanes are used as mobile devices, and a method and a system for estimating relative pose between mobile devices based on combination of IMU and data chain are described in detail. The positioning device is fixedly arranged on the airplane and consists of an IMU device and a data link device which are rigidly connected.

The inertial motion unit (Inertial Motion Unit, IMU) is composed of acceleration and a gyroscope sensor, can measure the acceleration and the angular velocity of the inertial motion unit, and is widely applied to the field of mobile equipment positioning.

The data link can be used to extrapolate the spatial distance between the signal transmitting device and the signal receiving device by calculating the time difference between the transmission of the signal to the signal reception. Meanwhile, a phase and difference monopulse measurement calculation method is adopted to calculate the space angle between the signal sender and the signal receiver, and the low-precision relative position between the receiving equipment and the sending equipment can be obtained through calculation.

After the positioning device is fixedly installed on an airplane, before the relative pose estimation method between mobile equipment based on the combination of the IMU and the data chain is executed, the relative pose between the IMU and the data chain and the measurement delay of the data chain are required to be calibrated:

the IMU coordinate system is used as a positioning equipment coordinate system, and the relative pose transformation of the data chain coordinate system and the IMU coordinate system is obtained through calibration by a manual measurement method

The delay exists in the measurement of the data link, the measurement delay calibration of the data link is needed before the use, and the measurement delay deltat of the data link is estimated.

The specific data chain calibration process comprises the following steps:

considering the time delay generated by the data link signal receiving and transmitting and signal processing, the calculation formula of the measurement distance between a pair of data links is as follows:

in the middle ofWhen the data chain a on one plane sends signals, the data chain b on the other plane replies to the signals of the data chain a after receiving the signals sent by the data chain a, and the time when the data chain a receives the reply signals is +.>c is the speed of light, δt is the delay of the data link receiving, transmitting and processing time.

When the data link is marked, δt can be obtained by solving the n ranging results and GNSS information at the corresponding moment, and the calculation formula is as follows:

d ^e ＝||T _el P ^a -T _el P ^b ||

d in ^m ，d ^e The measured value and the calculated value of the distance are respectively,and->The measured value and the calculated value of the distance at the moment i are respectively. T (T) _el Is a transformation matrix from a longitude and latitude high coordinate system to a geocentric ground rectangular coordinate system, P ^a ，P ^b The positioning device is used for sending the data link information and receiving the position information of the positioning device of the data link information under a longitude and latitude high coordinate system.

After calibration is completed, the two planes realize relative pose estimation between mobile devices based on IMU and data chain combination in the flight process, and in the whole positioning and relative pose estimation process, the positioning devices can communicate in real time, and the data acquired by the IMU and the self-position and pose information obtained by calculation are interacted.

The method comprises the following specific steps:

step 1: for each aircraft, the values of acceleration and angular velocity are acquired in real time through an on-board IMU device, and the position and attitude information of the aircraft itself under a navigation coordinate system is obtained by integrating the acceleration and the angular velocity through an inertial navigation calculation method (a gyroscope can measure the angular velocity of the carrier and an accelerometer can measure the acceleration of the carrier).

The specific inertial navigation calculation method comprises the following steps:

the acceleration information measured by the accelerometer is a value under a machine body coordinate system, the positioning information of the aircraft is required to be solved, the values are required to be converted into a navigation coordinate system, and the attitude transformation matrix from the machine body coordinate system to the navigation coordinate system can be obtained through the attitude angle measured by the gyroscope.

Step 1.1: and obtaining an attitude transformation matrix from the organism coordinate system at the moment k to the navigation coordinate system by using an attitude angle measured by the gyroscope at the moment k through the following processes:

before conversion, the attitude transformation matrix from the organism coordinate system at the current moment to the navigation coordinate system is needed to be solved, and the calculation method for obtaining the attitude of the airplane at the current moment is as follows:

wherein:is a gesture transformation matrix from a machine body coordinate system to a navigation coordinate system at the moment k, ⁿ T _k,k-1 the transformation matrix is a transformation matrix of the navigation coordinate system from k-1 time to k time, and the transformation matrix is a unit matrix. / >The transformation matrix from the machine body coordinate system to the navigation coordinate system at the moment k-1. />And measuring the gyroscope from the time k-1 to the time k to obtain an angular velocity integral solution for the pose change matrix of the machine body coordinate system from the time k-1 to the time k. Omega (τ) is the angular velocity measured by the gyroscope at τ, Δθ _k For the IMU pose rotation quantity from k-1 to k time _k And the attitude quantity of the IMU at the moment k under the navigation coordinate system.

Step 1.2: the acceleration a measured by an accelerometer on a machine body coordinate system at the moment k ^b Transformation matrix from machine body coordinate system at k moment to navigation coordinate systemConversion to a navigation coordinate system:

step 1.3: using the acceleration value in the navigation coordinate system obtained in the step 1.2 and using a velocity differential equation

Calculating the speed of the aircraft, whereIs the differential value of inertial navigation speed, f ⁿ For the acceleration measured on the navigation coordinate system, < >>Is a Ke type acceleration, wherein V ⁿ For the speed of the aircraft under the navigational coordinate system, +.>Is the rotational angular velocity of the earth in the navigation coordinate system. />Centripetal acceleration induced for carrier movement, wherein +.>Is the angular velocity of rotation of the aircraft around the earth in the navigation coordinate system. g ⁿ Is the gravitational acceleration in the navigation coordinate system. Sampling the increment information in each updating period, and then the speed of the airplane at the moment k is as follows:

In the middle ofSpeed of the aircraft in the navigation coordinate system at time k, k-1, < >>For the bits from the k-time machine system to the navigation coordinate systemGesture transformation matrix->The pose transformation matrix is the pose transformation matrix of the machine body coordinate system from the t moment to the k moment.

Step 1.4: the speed of the aircraft under the navigation coordinate system obtained in the step 1.3 is utilized, and an update equation of the aircraft position can be realized by adopting a simple trapezoidal integration method:

wherein P is _k-1 And P _k The positions of the aircraft in the navigation coordinate system at the time k-1 and the time k respectively comprise the latitude L, the longitude lambda and the altitude h of the aircraft. τ is the time interval between time k-1 and time k. M is M _pv(k-1/2) The matrix is a transformation matrix for converting the navigation coordinates at the moment of k-1/2 into the longitude and latitude high coordinates. R is R _M And R is _N The main curvature radiuses of the meridian circle and the mortise unitary circle of the airplane are respectively represented.

Step 2: and finishing the calculated position, speed and pose information of the airplane. And integrating the calculated speed, displacement and attitude information of the aircraft into a state information frame of the aircraft at each moment, and outputting the state information frame through a data chain.

Step 3: and establishing a data link distance calculation model, and calculating the distance between two airplanes through the time difference of data link information release and receiving. And calculating the position of the information sending device under the northeast coordinate system of the receiving device by using the azimuth angle and the altitude angle of the data link device when receiving the information.

In addition, in the positioning process, the distance value measured by the data chain is shielded and subjected to sensor conversion time interval, the measured distance value has step phenomenon, bad distance measured values are screened out by using a motion model, and the estimated distance value is used for replacing the bad distance measured values.

The specific process is as follows:

as described above, the measurement distance between a pair of data link sensors can be calculated according to the data link information transmission and reception time difference, and the calculation formula is as follows:

therefore, firstly, the dead point is removed by calculating the relative distance measured value at the continuous time and comparing the distance measured value variation between the adjacent time points:

δd _k-1 ＝d _k-1 -d _k-2

δd _k ＝d _k -d _k-1

d in _k-2 ,d _k-1 ,d _k The relative distance measurements at the k-2, k-1, k times, respectively. δd _k-1 Is the difference between the relative distance measurements at time k-2 and time k-1, δd _k For the difference between the relative distance measurement values at the kth time and the kth-1 time, Δd is the relative distance change threshold, and the continuous rest time δd should be taken into account _k-1 Case=0.

After eliminating dead pixels of the relative distance measurement value, decomposing the relative distance value to a northeast day coordinate system of a positioning device for receiving the data chain according to an azimuth angle alpha and an altitude angle gamma obtained by measuring the data chain, wherein the relative position obtained by decomposition is as follows:

In the middle ofThe coordinate values of x, y and z of the positioning device b under the northeast and north day coordinate system of the positioning device a are obtained through data chain measurement respectively.

Step 4: based on the step 1, the positioning equipment obtains the pose information of the positioning equipment through IMU-based calculation, and in the step, the positioning equipment transmits the pose information obtained through IMU-based calculation to other positioning equipment through a data link. After receiving the information, the positioning equipment at the receiving end analyzes the information and obtains a relative distance measurement value between the two airplanes based on data chain measurement by the method of the step 3.

Because the communication frequency of the data chain is different from the estimated pose frequency of the IMU, the relative pose between the estimated positioning devices needs to be calculated by combining the original data of the two sensors at the same moment, so that the time alignment of the state parameters and the data chain measurement needs to be realized, and a data frame containing the information of the state parameters and the data chain measurement needs to be constructed. Therefore, at the receiving end, the pose information and the data chain measurement information of the receiving end are time aligned by adopting a linear difference method based on the data chain measurement time.

The specific process is as follows:

constructing a data structure comprising the measuring moment of the data chain, the relative distance measured by the data chain and the measured relative angle: distance frames. Constructing a data structure containing the moment of inertial navigation pose calculation and the position and the pose of inertial navigation calculation: inertial navigation pose frames.

After receiving the data link information sent by the airplane b, the data link information of the airplane a calculates the relative distance and angle measured by the data link, constructs the relative distance and angle into a distance frame, and stores the distance frame into a queue of the distance frame. Meanwhile, inertial navigation information of the airplane b is contained in the data chain information, an inertial navigation pose frame is constructed, and the inertial navigation pose frame is stored in an inertial navigation pose frame queue.

With time of second distance frame receivedThe timestamp is the initial timestamp t of the system _D0 Taking the timestamp mean value of two adjacent inertial navigation pose frames before and after the second distance frame as the initial moment of the inertial navigation pose frame of the airplane for transmitting pose data:

the system time stamp of the following kth inertial navigation pose frame and the nth distance frame is as follows:

t is in _Ik Acquisition time stamp, t for kth inertial navigation pose frame _Dn Is the acquisition timestamp of the nth distance frame.

Searching two continuous inertial navigation pose frames meeting the conditions in the stored airplane inertial navigation pose frame queue:

in the middle ofAnd respectively carrying out linear difference on the inertial pose frames of the kth frame and the k+1 frame for the time stamps of the previous inertial pose frame and the next inertial pose frame when the nth distance frame is acquired, wherein the formula is as follows:

p in the formula ⁿ ，θ ⁿ The estimated position and attitude of the aircraft for inertial navigation after time alignment. P (P) ^k ，P ^k+1 And theta ^k ，θ ^k+1 The estimated position and attitude of the aircraft in the inertial navigation frames of the kth frame and the k+1 frame, respectively.

Step 5: after alignment of the positioning information data is completed, the pose information of the positioning equipment for transmitting information is arranged, the pose of the positioning equipment is converted into the machine body coordinate system of the information receiving equipment, and a relative position residual error is established by using the calculated relative position of the transmitting equipment under the machine body coordinate system of the information receiving equipment and the relative position of the transmitting equipment under the machine body coordinate system of the information receiving equipment, which is obtained by measuring a data chain.

Because the pose information of the positioning equipment in the positioning process is based on the navigation coordinate system of the respective positioning equipment, the measurement information of the data link is based on the northeast and north day coordinate system of each moment, and the relative position between the airplanes is based on the machine body coordinate system of each airplane, the relative position residual error can be established by integrating the coordinate systems.

The specific process is as follows:

step 5.1: and storing the latest m distance frames received and the inertial pose frames obtained by corresponding interpolation.

Step 5.2: establishing a position residual error:

in the middle ofAnd measuring the position of the transmitting positioning equipment b corresponding to the k-i frame state parameter in the machine body coordinate system of the receiving positioning equipment a for the data chain. / >The position of the transmitting positioning device b under the body coordinate system of the receiving positioning device a is estimated for inertial navigation. To ensure that the optimization problem is solvable, the value of mGreater than 2,0<i<m。

The data link measures the position of the transmitting positioning device b under the northeast and north day coordinate system of the k-i moment of the receiving positioning device a, and converts the position into the position of the transmitting positioning device b under the body coordinate system of the receiving positioning device a, and the calculation formula is as follows:

in the middle ofTo receive the pose transformation matrix of the positioning device a from the navigation coordinate system to the machine body coordinate system at the moment k-i,and sending the position of the positioning device b under the navigation coordinate system of the positioning device a for the moment k-i.

The calculation formula of the position of the inertial navigation estimated transmitting positioning device b under the body coordinate system of the positioning device a is as follows:

in the middle ofFor receiving the transformation matrix of the k-moment coordinate system of the positioning device a into the k-i-moment coordinate system,/>For transmitting the change matrix of the k-i time coordinate system of positioning device b to the k time coordinate system,/->For the relative pose of transmitting positioning device b at time k under the coordinate system of receiving positioning device a,/->The position of the positioning device b in its coordinate system of transmitting the positioning device b is transmitted for the moment k-i.

The aircraft pose obtained by using inertial navigation estimation is the relative pose under the navigation coordinate system Is calculated as follows:

in the middle ofAnd sending the position and orientation transformation matrix of the northeast and north day coordinate system of the positioning equipment b to the machine body coordinate system at the moment k.And (3) fixing the pose transformation matrix from the rectangular coordinate system to the northeast and north day coordinate system of the transmitting positioning equipment b for the moment k. />And (5) transmitting a pose transformation matrix from the north-east coordinate system of the positioning equipment b to the geocentric fixed coordinate system for the moment k-i. />And transmitting the machine system of the positioning equipment b to a pose transformation matrix under the north-east coordinate system of the positioning equipment b at the moment k-i.

Step 6: based on the time sequence, the relative position residuals at the first m moments are combined, a relative pose optimization equation is established, and the relative pose among positioning devices is optimized and calculated by adopting a Levenberg-Ma Kaer special method.

The specific process is as follows:

the relative position residual errors established at the first m times of the current time (the kth time) are weighted and summed, and a relative pose estimation optimization equation f (x) based on a time sequence is established as follows:

w in the formula ^k-i The weight of the k-i th relative position error,is the position residual established in step 5. The optimized variable x is the relative pose of the body coordinate system of the positioning equipment b at the current moment under the body coordinate system of the positioning equipment a.

And obtaining the relative pose information among the positioning devices by solving the optimization equation.

Based on the above method, the invention also provides a relative positioning system of the combination of the data chain and the IMU, which comprises the following steps: the system comprises an inertial navigation module, a data link measurement module, a flight state parameter, a data link measurement time alignment module and a relative pose estimation module; wherein:

and the inertial navigation module is used for solving the flight pose according to the acceleration and the angular velocity acquired by the accelerometer and the gyroscope, integrating the flight pose into a state parameter frame of the positioning equipment, and transmitting the obtained state parameter frame at each moment to other unmanned aerial vehicles through a data chain.

And the data chain measuring module is used for calculating the distance between the positioning devices according to the receiving and releasing time of the data chain information frames and removing the erroneous measured value by adopting a motion model. And according to the angle and the distance value when the data are linked, decomposing the distance to obtain the relative position of the positioning equipment b under the north-east coordinate system of the positioning equipment a.

And the flight state parameter and data link measurement time alignment module is used for carrying out linear interpolation on the pose estimated by the inertial navigation of the positioning equipment b based on the data link time stamp to obtain the flight state parameter at the measurement moment.

And the relative pose estimation module is used for converting the relative position measured by the data chain into a machine body coordinate system and calculating the relative position among the aircrafts at each moment according to the first m state parameters. And constructing a relative position residual error between the airplanes by using the relative position between the airplanes obtained by measuring the data chain and the relative position obtained by calculating the inertial navigation, and establishing a relative pose optimization equation between the unmanned planes based on the time sequence. And (5) performing iterative optimization by using a Levenberg-Ma Kaer special method to obtain the real-time relative pose between unmanned aerial vehicles.

Although embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the invention, and that variations, modifications, alternatives, and variations may be made in the above embodiments by those skilled in the art without departing from the spirit and principles of the invention.

Claims (8)

1. A relative pose estimation method between mobile devices combined by an IMU and a data chain is characterized by comprising the following steps: the mobile equipment is provided with positioning equipment, and the positioning equipment consists of an IMU (inertial measurement Unit) device and a data link device which are rigidly connected; in the relative pose estimation process, the positioning devices can communicate in real time, and the data acquired by respective IMU and the self-position and pose information obtained by respective calculation are interacted;

the relative pose estimation method comprises the following steps:

step 1: for each mobile device, acquiring the values of acceleration and angular velocity in real time through an IMU device installed on the mobile device, and integrating the acceleration and the angular velocity by using an inertial navigation calculation method to obtain the position and the attitude information of the mobile device under a navigation coordinate system;

step 2: integrating the calculated position, speed and pose information of the mobile equipment into a state information frame of the mobile equipment at each moment, and outputting the state information frame through a data chain;

Step 3: establishing a data link distance calculation model, and calculating the distance between two mobile devices through the time difference of data link information release and receiving; using the azimuth angle and the altitude angle of the data link equipment when receiving information, calculating the position of the information sending equipment under the northeast day coordinate system of the receiving equipment;

step 4: based on the step 1, the positioning equipment obtains the pose information of the positioning equipment through IMU-based calculation, and the positioning equipment transmits the pose information obtained through IMU-based calculation to other positioning equipment through a data link; after receiving the information, the positioning equipment at the receiving end analyzes the information and obtains a relative distance measurement value between the two mobile equipment based on data link measurement by the method of the step 3;

and at the receiving end, the pose information and the data chain measurement information of the receiving end are aligned in time by adopting a linear difference value based on the data chain measurement time:

constructing a data structure comprising the measuring moment of the data chain, the relative distance measured by the data chain and the measured relative angle: a distance frame; constructing a data structure containing the moment of inertial navigation pose calculation and the position and the pose of inertial navigation calculation: inertial navigation pose frames;

after receiving the data link information sent by the mobile device b, the data link receiving end of the mobile device a calculates the relative distance and angle measured by the data link, constructs the relative distance and angle into a distance frame, and stores the distance frame into a queue of the distance frame; meanwhile, inertial navigation information of the mobile equipment b is contained in the data chain information, an inertial navigation pose frame is constructed, and the inertial navigation pose frame is stored in an inertial navigation pose frame queue;

Taking the received time stamp of the second distance frame as the initial time stamp t of the system _D0 Taking the timestamp mean value of two adjacent inertial navigation pose frames before and after the second distance frame as the initial moment of the inertial navigation pose frame of the transmitting end:

the system time stamp of the following kth inertial navigation pose frame and the nth distance frame is as follows:

t is in _Ik Acquisition time stamp, t for kth inertial navigation pose frame _Dn An acquisition timestamp for the nth distance frame;

searching two consecutive inertial navigation pose frames meeting the conditions in the stored inertial navigation pose frame queue:

in the middle ofAnd respectively carrying out linear difference on the inertial pose frames of the kth frame and the k+1 frame for the time stamps of the previous inertial pose frame and the next inertial pose frame when the nth distance frame is acquired, wherein the formula is as follows:

obtaining P ⁿ ，θ ⁿ Inertial navigation estimation after time alignmentA position and a posture of the mobile device; p (P) ^k ，P ^k+1 And theta ^k ，θ ^k+1 The position and the gesture of the mobile device estimated in inertial navigation frames of the kth frame and the k+1th frame respectively;

step 5: after alignment of positioning information data is completed, pose information of positioning equipment for transmitting information is arranged, the pose of the positioning equipment is converted into a machine body coordinate system of receiving information equipment, and a relative position residual error is established by using the calculated relative position of the transmitting equipment under the machine body coordinate system of the receiving equipment and the relative position of the transmitting equipment under the machine body coordinate system of the receiving equipment, which is obtained by measuring a data chain;

Step 6: based on the time sequence, the relative position residuals at the first m moments are combined, a relative pose optimization equation is established, and the relative pose among positioning devices is optimized and calculated by adopting a Levenberg-Ma Kaer special method.

2. The method for estimating relative pose between mobile devices of IMU and data chain combination of claim 1, wherein: calibrating the relative pose between the IMU and the data chain: the IMU coordinate system is used as a positioning equipment coordinate system, and the relative pose transformation of the data chain coordinate system and the IMU coordinate system is obtained through calibration by a manual measurement method

3. The method for estimating relative pose between mobile devices of IMU and data chain combination of claim 1, wherein: calibrating the data chain measurement delay:

the calculation formula of the measurement distance between a pair of data chains is as follows:

in the middle ofWhen a signal is sent to the data link a on one mobile device, after the data link b on the other mobile device receives the signal sent by the data link a, the signal is reverted to the data link a signal, and the time when the data link a receives the reverted signal is +.>c is the speed of light, δt is the delay of the receiving, transmitting and processing time of the data chain;

and solving to obtain δt through the n ranging results and GNSS information at corresponding moments, wherein the calculation formula is as follows:

d ^e ＝||T _el P ^a -T _el P ^b ||

D in ^m ，d ^e The measured value and the calculated value of the distance are respectively,and->Respectively measuring and calculating the distance at the moment i; t (T) _el Is a transformation matrix from a longitude and latitude high coordinate system to a geocentric ground rectangular coordinate system, P ^a ，P ^b The positioning device is used for sending the data link information and receiving the position information of the positioning device of the data link information under a longitude and latitude high coordinate system.

4. The method for estimating relative pose between mobile devices of IMU and data chain combination of claim 1, wherein: in step 1, the inertial navigation calculation method is as follows:

step 1.1: and obtaining an attitude transformation matrix from the body coordinate system of the k-moment mobile equipment to the navigation coordinate system by using the attitude angle measured by the k-moment gyroscope through the following processes:

firstly, solving a gesture transformation matrix from a body coordinate system at the current moment to a navigation coordinate system, and obtaining the pose of the mobile equipment at the current moment, wherein the calculation method comprises the following steps:

wherein:is a gesture transformation matrix from a machine body coordinate system to a navigation coordinate system at the moment k, ⁿ T _k,k-1 the method comprises the steps of (1) setting a transformation matrix of a navigation coordinate system from k-1 time to k time, wherein the transformation matrix is a unit matrix; />The transformation matrix from the machine body coordinate system to the navigation coordinate system at the moment k-1; />The angular velocity integral solution is obtained by measuring a gyroscope from the moment k-1 to the moment k for the pose change matrix of the machine body coordinate system from the moment k-1 to the moment k; omega (τ) is the angular velocity measured by the gyroscope at τ, Δθ _k For the IMU pose rotation quantity from k-1 to k time _k The attitude quantity of the IMU at the moment k under the navigation coordinate system;

step 1.2: the acceleration a measured by an accelerometer on a machine body coordinate system at the moment k ^b Transformation matrix from machine body coordinate system at k moment to navigation coordinate systemConversion to a navigation coordinate system:

step 1.3: using the acceleration value in the navigation coordinate system obtained in the step 1.2 and using a velocity differential equation

Calculating a mobile device speed, whereIs the differential value of inertial navigation speed, f ⁿ For the acceleration measured on the navigation coordinate system, < >>Is a Ke type acceleration, wherein V ⁿ For the speed of the mobile device in the navigational coordinate system, < >>The rotation angular velocity of the earth in the navigation coordinate system; />Centripetal acceleration induced for carrier movement, wherein +.>The rotation angular velocity of the mobile device around the earth in a navigation coordinate system; g ⁿ Is the gravitational acceleration under a navigation coordinate system; sampling the increment information in each updating period, and then the speed of the mobile device at the moment k is as follows:

in the middle ofFor k, k-1 the mobile device speeds in the navigation coordinate system, +.>For the pose transformation matrix from the machine system at the time of k to the navigation coordinate system,/for the navigation coordinate system>The pose transformation matrix is a pose transformation matrix of the machine body coordinate system from the moment t to the moment k;

Step 1.4: and (3) utilizing the speed of the mobile equipment in the navigation coordinate system obtained in the step (1.3), and adopting a trapezoidal integration method to realize an update equation of the position of the mobile equipment:

wherein P is _k-1 And P _k The positions of the mobile equipment at the time k-1 and the time k under a navigation coordinate system respectively comprise latitude L, longitude lambda and altitude h of the mobile equipment; τ is the time interval between time k-1 and time k; m is M _pv(k-1/2) The matrix is a transformation matrix for converting the navigation coordinates at the moment of k-1/2 into longitude and latitude high coordinates; r is R _M And R is _N Main curve respectively representing meridian circle and mortise circle of mobile deviceRadius of the rate.

5. The method for estimating relative pose between mobile devices of IMU and data chain combination of claim 1, wherein: in step 3, bad distance measurements are screened out by:

the dead pixel is removed by calculating the relative distance measured value at the continuous time and comparing the distance measured value variation between the adjacent time:

δd _k-1 ＝d _k-1 -d _k-2

δd _k ＝d _k -d _k-1

d in _k-2 ,d _k-1 ,d _k Relative distance measurements at the k-2, k-1, k times, respectively; δd _k-1 Is the difference between the relative distance measurements at time k-2 and time k-1, δd _k For the difference between the relative distance measurement values at the kth time and the kth-1 time, Δd is the relative distance change threshold, and the continuous rest time δd should be taken into account _k-1 Case=0.

6. The method for estimating relative pose between mobile devices of IMU and data chain combination of claim 1, wherein: in step 5, the relative position residual error is established after the coordinate system is unified, and the specific process is as follows:

step 5.1: storing the latest m distance frames received and the inertial pose frames obtained by corresponding interpolation;

step 5.2: establishing a position residual error:

in the middle ofMeasuring a data chain to obtain the position of the transmitting positioning equipment b corresponding to the k-i frame state parameter under the body coordinate system of the receiving positioning equipment a; />The position of the transmitting positioning equipment b under the body coordinate system of the receiving positioning equipment a is estimated for inertial navigation;

the data link measures the position of the transmitting positioning device b under the northeast and north day coordinate system of the k-i moment of the receiving positioning device a, and converts the position into the position of the transmitting positioning device b under the body coordinate system of the receiving positioning device a, and the calculation formula is as follows:

in the middle ofTo receive the pose transformation matrix of the positioning device a from the navigation coordinate system to the machine body coordinate system at the moment k-i,transmitting the position of the positioning device b under the navigation coordinate system of the positioning device a for the moment k-i;

the calculation formula of the position of the inertial navigation estimated transmitting positioning device b under the body coordinate system of the positioning device a is as follows:

In the middle ofFor receiving the transformation matrix of the k-moment coordinate system of the positioning device a into the k-i-moment coordinate system,/>For transmitting the change matrix of the k-i time coordinate system of positioning device b to the k time coordinate system,/->For the relative pose of transmitting positioning device b at time k under the coordinate system of receiving positioning device a,/->Transmitting the position of the positioning device b under the coordinate system of the transmitting positioning device b for the moment k-i;

the pose of the mobile equipment obtained by using inertial navigation estimation is the relative pose under the navigation coordinate system, then Is calculated as follows:

in the middle ofTransmitting a pose transformation matrix from the northeast coordinate system of the positioning equipment b to the machine body coordinate system at the moment k; />Fixing the pose transformation matrix from the rectangular coordinate system to the northeast and north day coordinate system of the transmitting and positioning equipment b for the earth center at the moment k; />Transmitting a pose transformation matrix from a north-east coordinate system of the positioning equipment b to a geocentric fixed coordinate system for the moment k-i; />And transmitting the machine system of the positioning equipment b to a pose transformation matrix under the north-east coordinate system of the positioning equipment b at the moment k-i.

7. The method for estimating relative pose between mobile devices of IMU and data chain combination of claim 1, wherein: the specific process of the step 6 is as follows:

and (3) carrying out weighted summation on the relative position residuals established at the first m times of the current kth time, and establishing a relative pose estimation optimization equation f (x) based on a time sequence, wherein the relative pose estimation optimization equation f (x) is as follows:

W in the formula ^k-i The weight of the k-i th relative position error,the position residual error established in the step 5 is obtained; the optimized variable x is the relative pose of the body coordinate system of the positioning equipment b at the current moment under the body coordinate system of the positioning equipment a.

8. A relative positioning system between mobile devices of IMU and data link combination, characterized in that: comprising the following steps: the system comprises an inertial navigation module, a data link measurement module, a flight state parameter, a data link measurement time alignment module and a relative pose estimation module; wherein:

the inertial navigation module is used for solving the flight pose according to the acceleration and the angular velocity acquired by the accelerometer and the gyroscope, integrating the flight pose into a state parameter frame of the positioning equipment, and transmitting the obtained state parameter frame at each moment to other mobile equipment through a data chain;

and the data chain measuring module is used for calculating the distance between the positioning devices according to the receiving and releasing time of the data chain information frames and removing the erroneous measured value by adopting a motion model. According to the angle and the distance value when the data are linked, decomposing the distance to obtain the relative position of the positioning equipment b under the north-east coordinate system of the positioning equipment a;

the flight state parameter and data link measurement time alignment module is used for carrying out linear interpolation on the pose estimated by the inertial navigation of the positioning equipment b based on the data link time stamp to obtain the flight state parameter at the measurement moment;

The relative pose estimation module converts the relative position measured by the data chain into a machine body coordinate system, and calculates the relative position between the mobile devices at each moment according to the first m state parameters; using the relative position between the mobile devices obtained by measuring the data chain and the relative position obtained by calculating the inertial navigation to construct a residual error of the relative position between the airplanes, and establishing a relative pose optimization equation between the mobile devices based on a time sequence; and (5) performing iterative optimization by using a Levenberg-Ma Kaer special method to obtain the real-time relative pose between the mobile devices.