Relative pose estimation method and system between mobile devices combined with IMU and data link

Abstract

The present invention proposes a method and system for estimating the relative posture between mobile devices in combination with an IMU and a data link. A rigid IMU sensor and a data link device are installed on the mobile device. After calibration, the position and posture information of the mobile device itself in a navigation coordinate system is obtained by the IMU device using an inertial navigation calculation method, and the information is integrated into a status information frame and output through the data link. The distance between the two mobile devices is calculated based on the time difference between the release and reception of data link information. The position of the information sending device in the northeast celestial coordinate system of the receiving device is calculated using the azimuth and altitude angle when the data link device receives the information. A relative position residual is established using the calculated relative position of the sending device in the body coordinate system of the receiving device and the relative position of the sending device in the body coordinate system of the receiving device measured by the data link. Based on the time series, a relative posture optimization equation is established to optimize the calculation of the relative posture between the positioning devices.

Description

Technical Field

[0001] The present invention relates to the field of autonomous positioning of mobile devices and relative positioning of devices, and in particular to a method and system for estimating relative posture between mobile devices by combining an IMU and a data link. Background Art

[0002] In recent years, mobile device swarms have become a major development in mobile robotics. Accurate positioning information is crucial for swarms to reliably execute tasks, and accurately estimating the relative poses of mobile devices within a swarm in real time is a key technology for swarm positioning.

[0003] Solutions for estimating the relative pose between mobile device clusters can be categorized as either external sensor-based or self-sensor-based. External sensor-based solutions primarily rely on GNSS positioning devices that provide absolute latitude and longitude information, achieving centimeter-level accuracy. Currently, this approach is used by most large-scale outdoor drone clusters. In GNSS-denied environments, optical capture systems or local positioning systems based on ultra-wideband (UWB) are often used to achieve relative positioning between devices within a mobile device cluster. However, these solutions are only suitable for use indoors or in small scenarios, severely limiting the mission capabilities of mobile devices.

[0004] As the mission scenarios of mobile devices become increasingly complex, relative positioning solutions based on their own sensors have become a hot topic in the current research on relative positioning between clusters due to their flexibility and ease of implementation. Existing relative positioning algorithms mainly rely on object recognition methods. For example, Yan Fei proposed a relative pose estimation method for quadcopters based on the detection of luminous rings. This method is implemented by identifying the luminous rings on the drones and calculating the relative pose between drones. This method is only suitable for small-scale unmanned equipment clusters. Xie Nianhao proposed a relative pose estimation method for drone formations based only on monocular vision information. Each drone is equipped with markers of different geometric shapes, and the relative pose between drones is estimated by identifying the markers. This method requires modification of mobile device hardware and can only be applied to mission scenarios where the distance between unmanned equipment in the cluster is relatively close. Summary of the Invention

[0005] To solve the problems existing in the prior art, the present invention proposes a method and system for estimating the relative pose between mobile devices based on an inertial motion unit (IMU) and a data link. The IMU and the data link device are rigidly connected to the positioning device. By measuring the motion of the positioning device and the relative position between the positioning devices, the relative pose between the mobile devices can be obtained with high precision.

[0006] The technical solution of the present invention is:

[0007] A method for estimating relative pose between mobile devices using an IMU and a data link, wherein the mobile device is equipped with a positioning device, the positioning device comprising a rigidly connected IMU device and a data link device; during the relative pose estimation process, the positioning devices can communicate in real time, exchanging data collected by their respective IMUs and their own calculated position and pose information;

[0008] The relative pose estimation method comprises the following steps:

[0009] Step 1: For each mobile device, use the IMU device installed on it to collect the acceleration and angular velocity values in real time. Use the inertial navigation calculation method to integrate the acceleration and angular velocity to obtain the position and attitude information of the mobile device itself in the navigation coordinate system;

[0010] Step 2: Integrate the calculated position, velocity, and posture information of the mobile device into a state information frame of the mobile device at each moment and output it through the data link;

[0011] Step 3: Establish a data link distance calculation model. Calculate the distance between the two mobile devices based on the time difference between sending and receiving data link information. Calculate the position of the sending device in the northeast celestial coordinate system of the receiving device using the azimuth and altitude angles of the data link device when receiving the information.

[0012] Step 4: Based on step 1, the positioning device obtains its own position information through IMU calculation. The positioning device transmits the position information calculated by the IMU to other positioning devices through the data link. After receiving the information, the positioning device at the receiving end analyzes the information and obtains the relative distance measurement value between the two mobile devices based on the data link measurement according to the method in step 3.

[0013] At the receiving end, the linear difference method is used based on the data link measurement time to align the time of its own posture information and the data link measurement information:

[0014] Construct a data structure containing the data link measurement time, the relative distance measured by the data link, and the relative angle measured: the distance frame; construct a data structure containing the inertial navigation posture calculation time, the position and posture calculated by the inertial navigation: the inertial navigation posture frame;

[0015] After receiving the data link information sent by mobile device b, the data link receiving end of mobile device a calculates the relative distance and angle measured by this data link, constructs it into a distance frame, and stores it in the distance frame queue; at the same time, it parses the inertial navigation information of mobile device b contained in the data link information, constructs an inertial navigation pose frame, and stores it in the inertial navigation pose frame queue;

[0016] The timestamp of the second distance frame received is used as the initial timestamp of the system. D0 , the average of the timestamps of the two adjacent inertial navigation pose frames before and after the second range frame is taken as the initial time of the inertial navigation pose frame at the transmitter:

[0017]

[0018] The system timestamps of the subsequent k-th inertial navigation pose frame and n-th distance frame are:

[0019]

[0020]

[0021] Where t Ik is the acquisition timestamp of the kth inertial navigation pose frame, t Dn is the acquisition timestamp of the nth range frame;

[0022] Retrieve two consecutive inertial navigation pose frames that meet the conditions in the saved inertial navigation pose frame queue:

[0023]

[0024] In the formula The timestamps of the previous and next inertial navigation pose frames when collecting the nth range frame are respectively used. The linear difference between the kth frame and the k+1th inertial pose frame is performed. The formula is as follows:

[0025]

[0026]

[0027] Get P n ,θ n The position and attitude of the mobile device estimated by inertial navigation after time alignment; P k , P k+1 and θ k ,θ k+1 are the estimated position and attitude of the mobile device in the inertial navigation frames of the kth frame and the k+1th frame respectively;

[0028] Step 5: After completing the alignment of the positioning information data, the posture information of the positioning device sending the information is sorted and its posture is converted to the body coordinate system of the receiving device. The relative position residual of the sending device in the body coordinate system of the receiving device is established by using the calculated relative position of the sending device in the body coordinate system of the receiving device and the relative position of the sending device in the body coordinate system of the receiving device measured by the data link;

[0029] Step 6: Based on the time series, the relative position residuals of the previous m moments are combined to establish the relative pose optimization equation, and the Levenberg-Macart method is used to optimize the relative pose between positioning devices.

[0030] Furthermore, the relative posture between the IMU and the data link is calibrated: the IMU coordinate system is used as the positioning device coordinate system, and the relative posture transformation between the data link coordinate system and the IMU coordinate system is obtained by manual measurement.

[0031] Furthermore, the data link measurement delay is calibrated:

[0032] The calculation formula for the measured distance between a pair of data links is:

[0033]

[0034] In the formula The moment when data link a on a mobile device sends a signal, data link b on another mobile device receives the signal sent by data link a and replies to data link a. The moment when data link a receives the reply signal is c is the speed of light, δt is the delay in sending, receiving and processing the data link;

[0035] Through n ranging results and the GNSS information at the corresponding time, δt is obtained by solving the following formula:

[0036]

[0037] d e =||T el P a -T el P b ||

[0038]

[0039] Where d m , d e are the measured and calculated values of the distance, and are the measured and calculated values of the distance at time i; T el is the transformation matrix from the latitude and longitude coordinate system to the Earth-centered Earth-fixed rectangular coordinate system, P a , P b They are the position information of the positioning device that sends data link information and the positioning device that receives data link information in the longitude and latitude coordinate system.

[0040] Furthermore, in step 1, the inertial navigation calculation method is as follows:

[0041] Step 1.1: Using the attitude angle measured by the gyroscope at time k, the attitude transformation matrix from the mobile device's body coordinate system to the navigation coordinate system at time k is obtained through the following process:

[0042] First, solve the attitude transformation matrix from the current body coordinate system to the navigation coordinate system, and obtain the calculation method of the current mobile device posture as follows:

[0043]

[0044]

[0045]

[0046] Where: is the attitude transformation matrix from the body coordinate system to the navigation coordinate system at time k, n T k,k-1 is the transformation matrix of the navigation coordinate system from time k-1 to time k, and this transformation matrix is the unit matrix; is the transformation matrix from the body coordinate system to the navigation coordinate system at time k-1; is the posture change matrix of the body coordinate system from time k-1 to time k, and is solved by integrating the angular velocity measured by the gyroscope from time k-1 to time k; ω(τ) is the angular velocity measured by the gyroscope at time τ, Δθ k is the IMU posture rotation from k-1 to k, φ k is the attitude of the IMU in the navigation coordinate system at time k;

[0047] Step 1.2: The acceleration a measured by the accelerometer in the body coordinate system at time k is b , through the transformation matrix from the body coordinate system to the navigation coordinate system at time k Convert to the navigation coordinate system:

[0048]

[0049] Step 1.3: Using the acceleration value in the navigation coordinate system obtained in step 1.2, use the velocity differential equation

[0050]

[0051] Calculate the speed of the mobile device, where is the differential value of the inertial navigation velocity, f n is the acceleration measured in the navigation coordinate system, is the Coriolis acceleration, where V n is the speed of the mobile device in the navigation coordinate system, is the angular velocity of the Earth in the navigation coordinate system; is the centripetal acceleration caused by the carrier motion, where is the angular velocity of the mobile device around the earth in the navigation coordinate system; g n is the gravitational acceleration in the navigation coordinate system; for the incremental information sampling in each update cycle, the speed of the mobile device at time k is:

[0052]

[0053] In the formula is the speed of the mobile device in the navigation coordinate system at time k, k-1, is the pose transformation matrix from the machine system to the navigation coordinate system at time k, is the pose transformation matrix of the body coordinate system from time t to time k;

[0054] Step 1.4: Using the velocity of the mobile device in the navigation coordinate system obtained in step 1.3, the trapezoidal integration method can be used to implement the update equation of the mobile device position:

[0055]

[0056]

[0057] Among them, P k-1 With P k are the positions of the mobile device in the navigation coordinate system at time k-1 and time k, including the latitude L, longitude λ, and altitude h of the mobile device; τ is the time interval between time k-1 and time k; M pv(k-1 / 2) The matrix is the transformation matrix from navigation coordinates at time k-1 / 2 to longitude and latitude coordinates; R M and R N They respectively represent the principal curvature radii of the meridian and the meridian at the location of the mobile device.

[0058] Furthermore, in step 3, bad distance measurements are filtered out by the following process:

[0059] By calculating the relative distance measurement values at consecutive moments and comparing the distance measurement value changes between adjacent moments, bad measurement points can be eliminated:

[0060] δd k-1 =d k-1 -d k-2

[0061] δd k =d k -d k-1

[0062]

[0063] Where d k-2,d k-1 ,d k are the relative distance measurements at the k-2, k-1, and k moments respectively; δd k-1 is the difference between the relative distance measurements at time k-2 and time k-1, δd k is the difference between the relative distance measurement values at the kth moment and the k-1th moment, Δd is the relative distance change threshold, and the response time is δd. k-1 =0.

[0064] Furthermore, in step 5, the coordinate system is transformed to establish the relative position residual. The specific process is as follows:

[0065] Step 5.1: Save the latest m distance frames received and the corresponding interpolated inertial pose frames;

[0066] Step 5.2: Create position residuals:

[0067]

[0068] In the formula The position of the sending positioning device b in the body coordinate system of the receiving positioning device a corresponding to the state parameter of the ki-th frame measured by the data link; The position of the sending positioning device b in the body coordinate system of the receiving positioning device a obtained by inertial navigation estimation;

[0069] The data link measures the position of the transmitting positioning device b in the northeastern sky coordinate system at time ki of the receiving positioning device a, and converts it into the position of the transmitting positioning device b in the body coordinate system of the receiving positioning device a. The calculation formula is as follows:

[0070]

[0071] In the formula To receive the pose transformation matrix of the positioning device a from the navigation coordinate system to the body coordinate system at time ki, Send the position of positioning device b in the navigation coordinate system of positioning device a at time ki;

[0072] The calculation formula for the position of the sending positioning device b in the body coordinate system of the positioning device a estimated by inertial navigation is as follows:

[0073]

[0074] In the formula is the transformation matrix from the k-time coordinate system of the receiving positioning device a to the ki-time coordinate system, To send the change matrix of the coordinate system of positioning device b at time ki to the coordinate system at time k, is the relative position of the sending positioning device b in the coordinate system of the receiving positioning device a at time k, The position of the positioning device b in the coordinate system of the positioning device b is sent at time ki;

[0075] The pose of the mobile device estimated using inertial navigation is the relative pose in the navigation coordinate system. The calculation of is as follows:

[0076]

[0077]

[0078]

[0079]

[0080] In the formula The pose transformation matrix from the northeast celestial coordinate system of positioning device b to the body coordinate system is sent at time k; is the pose transformation matrix from the Earth-centered Earth-fixed rectangular coordinate system to the Northeast Sky coordinate system of the sending positioning device b at time k; The pose transformation matrix from the north-east coordinate system of positioning device b to the Earth-centered Earth-fixed coordinate system is sent at time ki; is the pose transformation matrix from the machine system of the sending positioning device b to the north-east coordinate system of the sending positioning device b at time ki.

[0081] Furthermore, the specific process of step 6 is as follows:

[0082] The weighted sum of the relative position residuals established at the m moments before the current k-th moment is used to establish the relative pose estimation optimization equation f(x) based on the time series, as follows:

[0083]

[0084] Where W k-i is the weight of the ki-th relative position error, is the position residual established in step 5; the optimized variable x is the relative position of the body coordinate system of positioning device b in the body coordinate system of positioning device a at the current moment.

[0085] A relative positioning system between mobile devices combining an IMU and a data link, comprising: an inertial navigation module, a data link measurement module, a flight state parameter and data link measurement time alignment module, and a relative posture estimation module; wherein:

[0086] The inertial navigation module calculates the flight posture based on the acceleration and angular velocity collected by the accelerometer and gyroscope and integrates it into the state parameter frame of the positioning device. The state parameter frame obtained at each moment is transmitted to other mobile devices through the data link;

[0087] The data link measurement module calculates the distance between the positioning devices based on the reception and release times of the data link information frames, using a motion model to eliminate erroneous measurements. Based on the angle and distance values at the time of data link reception, the distance is decomposed to determine the relative position of positioning device b in the north-east coordinate system of positioning device a.

[0088] The flight state parameter and data link measurement time alignment module performs linear interpolation on the inertial navigation estimated pose of positioning device b based on the data link timestamp to obtain the flight state parameters at the measurement time;

[0089] The relative pose estimation module converts the relative position measured by the data link into the body coordinate system, and calculates the relative position between the mobile devices at each moment based on the first m state parameters; uses the relative position between the mobile devices measured by the data link and the relative position calculated by inertial navigation to construct the relative position residual between aircraft, and establishes the relative pose optimization equation between mobile devices based on time series; uses the Levenberg-Macartt method to iteratively optimize and obtain the real-time relative pose between mobile devices.

[0090] Beneficial effects

[0091] Compared with the prior art, the advantages of the present invention are:

[0092] The method of the present invention can achieve high-precision relative posture estimation between mobile devices by relying only on inertial navigation equipment and a data link that can measure distance and angle.

[0093] The present invention does not require changing the structure of the mobile device, does not use a mobile device identification algorithm based on shape features, and can perform relative posture estimation between mobile devices at a long distance.

[0094] The present invention adopts a method of joint optimization of inertial navigation positioning information and data link measurement information, which can realize autonomous positioning of mobile devices and relative posture estimation between mobile devices in a GNSS-denied environment.

[0095] Additional aspects and advantages of the present invention will be set forth in part in the description which follows and, in part, will be obvious from the description which follows, or may be learned by practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0096] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments with reference to the accompanying drawings, in which:

[0097] Figure 1 Flowchart of the relative positioning method between mobile devices combining data link and IMU. DETAILED DESCRIPTION

[0098] The following describes in detail embodiments of the present invention. The embodiments are exemplary and intended to explain the present invention, but are not to be construed as limiting the present invention.

[0099] like Figure 1 As shown, this embodiment uses two aircraft as mobile devices and describes in detail a method and system for estimating relative pose between mobile devices based on an IMU and data link combination. A positioning device is fixedly installed on the aircraft, and the positioning device consists of a rigidly connected IMU device and a data link device.

[0100] An inertial motion unit (IMU) consists of accelerometer and gyroscope sensors, which can measure its own acceleration and angular velocity and is widely used in the field of mobile device positioning.

[0101] By calculating the time difference between signal transmission and signal reception, the data link can infer the spatial distance between the signal transmitting device and the signal receiving device. Phase and difference single pulse measurement and calculation methods can also be used to calculate the spatial angle between the signal transmitting device and the signal receiving device. This calculation can be used to obtain the low-precision relative position between the receiving device and the transmitting device.

[0102] After the positioning device is fixedly installed on the aircraft, before executing the relative pose estimation method between mobile devices based on the combination of IMU and data link, it is necessary to calibrate the relative pose between the IMU and the data link and the data link measurement delay:

[0103] Using the IMU coordinate system as the positioning device coordinate system, the relative position transformation between the data link coordinate system and the IMU coordinate system is calibrated by manual measurement.

[0104] There is a delay during data link measurement. Data link measurement delay calibration is required before use to estimate the data link measurement delay δt.

[0105] The specific data link calibration process is as follows:

[0106] Taking into account the time delay caused by data link signal reception and transmission and signal processing, the calculation formula for the measured distance between a pair of data links is:

[0107]

[0108] In the formula The time when data link a on one aircraft sends a signal, and data link b on another aircraft receives the signal sent by data link a and replies to data link a. The time when data link a receives the reply signal is c is the speed of light, and δt is the delay in sending, receiving, and processing the data link.

[0109] During data link calibration, δt can be calculated using n ranging results and the GNSS information at the corresponding time. The calculation formula is as follows:

[0110]

[0111] d e =||T el P a -T el P b ||

[0112]

[0113] Where d m , d e are the measured and calculated values of the distance, and are the measured and calculated values of the distance at time i respectively. el is the transformation matrix from the latitude and longitude coordinate system to the Earth-centered Earth-fixed rectangular coordinate system, P a , P b They are the position information of the positioning device that sends data link information and the positioning device that receives data link information in the longitude and latitude coordinate system.

[0114] After calibration, the two aircraft can achieve relative pose estimation between mobile devices based on the combination of IMU and data link through the following process during flight. During the entire positioning and relative pose estimation process, the positioning devices can communicate in real time, exchanging data collected by their respective IMUs and their own calculated position and attitude information.

[0115] The specific steps are as follows:

[0116] Step 1: For each aircraft, use the onboard IMU device to collect the values of acceleration and angular velocity in real time. Use the inertial navigation calculation method to integrate the acceleration and angular velocity to obtain the aircraft's own position and attitude information in the navigation coordinate system (the gyroscope can measure the carrier's angular velocity, and the accelerometer can measure the carrier's acceleration).

[0117] The specific inertial navigation calculation method is as follows:

[0118] The acceleration information measured by the accelerometer is in the body coordinate system. To determine the aircraft's positioning information, these values need to be converted to the navigation coordinate system. The attitude angle measured by the gyroscope can be used to obtain the attitude transformation matrix from the body coordinate system to the navigation coordinate system.

[0119] Step 1.1: Using the attitude angle measured by the gyroscope at time k, the attitude transformation matrix from the body coordinate system to the navigation coordinate system at time k is obtained through the following process:

[0120] Before conversion, it is necessary to solve the attitude transformation matrix from the current body coordinate system to the navigation coordinate system, and the calculation method for obtaining the current aircraft posture is as follows:

[0121]

[0122]

[0123]

[0124] Where: is the attitude transformation matrix from the body coordinate system to the navigation coordinate system at time k, n T k,k-1 is the transformation matrix of the navigation coordinate system from time k-1 to time k, and this transformation matrix is the unit matrix. is the transformation matrix from the body coordinate system to the navigation coordinate system at time k-1. is the pose change matrix of the body coordinate system from time k-1 to time k, and is solved by integrating the angular velocity measured by the gyroscope from time k-1 to time k. ω(τ) is the angular velocity measured by the gyroscope at time τ, Δθ k is the IMU posture rotation from k-1 to k, φ k is the attitude of the IMU in the navigation coordinate system at time k.

[0125] Step 1.2: The acceleration a measured by the accelerometer in the body coordinate system at time k is b , through the transformation matrix from the body coordinate system to the navigation coordinate system at time k Convert to the navigation coordinate system:

[0126]

[0127] Step 1.3: Using the acceleration value in the navigation coordinate system obtained in step 1.2, use the velocity differential equation

[0128]

[0129] Calculate the aircraft speed, where is the differential value of the inertial navigation velocity, f nis the acceleration measured in the navigation coordinate system, is the Coriolis acceleration, where V n is the speed of the aircraft in the navigation coordinate system, is the angular velocity of the Earth in the navigation coordinate system. is the centripetal acceleration caused by the carrier motion, where is the angular velocity of the aircraft around the earth in the navigation coordinate system. g n is the gravity acceleration in the navigation coordinate system. Sampling the incremental information in each update cycle, the speed of the aircraft at time k is:

[0130]

[0131] In the formula is the speed of the aircraft in the navigation coordinate system at time k, k-1, is the pose transformation matrix from the machine system to the navigation coordinate system at time k, is the pose transformation matrix of the body coordinate system from time t to time k.

[0132] Step 1.4: Using the aircraft's velocity in the navigation coordinate system obtained in step 1.3, a simple trapezoidal integration method can be used to implement the aircraft position update equation:

[0133]

[0134]

[0135] Among them, P k-1 With P k are the positions of the aircraft in the navigation coordinate system at time k-1 and time k, including the aircraft's latitude L, longitude λ, and altitude h. τ is the time interval between time k-1 and time k. pv(k-1 / 2) The matrix is the transformation matrix from navigation coordinates at time k-1 / 2 to longitude and latitude coordinates. M and R N They respectively represent the main curvature radii of the meridian and the meridian at the aircraft's location.

[0136] Step 2: Organize the calculated aircraft position, velocity, and attitude information. The calculated aircraft velocity, displacement, and attitude information are integrated into a state information frame of the aircraft at each moment and output through the data link.

[0137] Step 3: Build a data link distance calculation model. Calculate the distance between the two aircraft using the time difference between sending and receiving data link information. Use the azimuth and altitude angles at the time the data link equipment receives the information to calculate the position of the sending device in the northeast celestial coordinate system of the receiving device.

[0138] In addition, during the positioning process, the distance value measured by the data link is affected by obstruction and the sensor conversion time interval, and the measured distance value has a step phenomenon. The motion model is used to screen out the poor distance measurement value, and the estimated distance value is used to replace the poor distance measurement value.

[0139] The specific process is as follows:

[0140] As mentioned above, the measurement distance between a pair of data link sensors can be calculated based on the time difference between sending and receiving data link information. The calculation formula is:

[0141]

[0142] Therefore, we first calculate the relative distance measurement values at consecutive moments and compare the distance measurement value changes between adjacent moments to eliminate bad measurement points:

[0143] δd k-1 =d k-1 -d k-2

[0144] δd k =d k -d k-1

[0145]

[0146] Where d k-2 ,d k-1 ,d k The relative distance measurement values at the k-2, k-1, and k moments respectively. k-1 is the difference between the relative distance measurements at time k-2 and time k-1, δd k is the difference between the relative distance measurement values at the kth moment and the k-1th moment, Δd is the relative distance change threshold, and the response time is δd. k-1 =0.

[0147] After removing the bad points in the relative distance measurement value, the relative distance value is decomposed into the northeast sky coordinate system of the positioning device receiving the data link according to the azimuth angle α and altitude angle γ measured by the data link. The relative position obtained by decomposition is:

[0148]

[0149]

[0150]

[0151] In the formula are the x, y, and z coordinate values of positioning device b in the northeast celestial coordinate system of positioning device a obtained through data link measurement.

[0152] Step 4: Based on the IMU-based position information calculated by the positioning device in Step 1, this device transmits this IMU-based position information to the other positioning device via the data link. The receiving positioning device interprets this information and, using the method from Step 3, measures the relative distance between the two aircraft via the data link.

[0153] Because the data link communication frequency differs from the IMU's estimated pose frequency, estimating the relative pose between positioning devices requires combining the raw data from both sensors at the same moment. This necessitates time alignment of state parameters with data link measurements and the construction of a data frame containing both. Therefore, at the receiving end, a linear interpolation method based on the data link measurement time is used to time-align the receiver's own pose information with the data link measurement information.

[0154] The specific process is as follows:

[0155] Construct a data structure containing the data link measurement time, the relative distance measured by the data link, and the relative angle measured: the distance frame. Construct a data structure containing the inertial navigation pose calculation time, the position and attitude calculated by the inertial navigation: the inertial navigation pose frame.

[0156] After receiving the data link information from aircraft B, aircraft A's data link receiver calculates the relative distance and angle measured in this data link, constructs a range frame, and stores it in the range frame queue. Simultaneously, it parses the inertial navigation information contained in the data link information, constructs an inertial navigation pose frame, and stores it in the inertial navigation pose frame queue.

[0157] The timestamp of the second distance frame received is used as the initial timestamp of the system. D0 , the average of the timestamps of the two adjacent inertial navigation pose frames before and after the second range frame is taken as the initial time of the inertial navigation pose frame of the aircraft sending the pose data:

[0158]

[0159] The system timestamps of the subsequent k-th inertial navigation pose frame and n-th distance frame are:

[0160]

[0161]

[0162] Where t Ik is the acquisition timestamp of the kth inertial navigation pose frame, t Dn The acquisition timestamp of the nth range frame.

[0163] Retrieve two consecutive inertial navigation pose frames that meet the conditions in the saved aircraft inertial navigation pose frame queue:

[0164]

[0165] In the formula The timestamps of the previous and next inertial navigation pose frames when collecting the nth range frame are respectively used. The linear difference between the kth frame and the k+1th inertial pose frame is performed. The formula is as follows:

[0166]

[0167]

[0168] Where P n ,θ n The position and attitude of the aircraft estimated by inertial navigation after time alignment. k , P k+1 and θ k ,θ k+1 are the estimated position and attitude of the aircraft in the k-th frame and the k+1-th frame of inertial navigation, respectively.

[0169] Step 5: After completing the alignment of the positioning information data, organize the posture information of the positioning device sending the information, and convert its posture to the body coordinate system of the receiving information device. Use the calculated relative position of the sending device in the body coordinate system of the receiving device and the relative position of the sending device in the body coordinate system of the receiving device measured by the data link to establish a relative position residual.

[0170] Since the position information of the positioning devices during the positioning process is based on the navigation coordinate system of each positioning device, the measurement information of the data link is based on the northeast sky coordinate system at each moment, and the relative positions between aircraft are based on the body coordinate system of each aircraft, the coordinate systems also need to be unified to establish the relative position residual.

[0171] The specific process is as follows:

[0172] Step 5.1: Save the latest m distance frames received and the corresponding interpolated inertial pose frames.

[0173] Step 5.2: Create position residuals:

[0174]

[0175] In the formula The position of the sending positioning device b in the body coordinate system of the receiving positioning device a corresponding to the state parameter of the ki-th frame is obtained by data link measurement. is the position of the sending positioning device b in the body coordinate system of the receiving positioning device a estimated by inertial navigation. In order to ensure that the optimization problem can be solved, the value of m should be greater than 2,0 <i<m。

[0176] The data link measures the position of the transmitting positioning device b in the northeastern sky coordinate system at time ki of the receiving positioning device a, and converts it into the position of the transmitting positioning device b in the body coordinate system of the receiving positioning device a. The calculation formula is as follows:

[0177]

[0178] In the formula To receive the pose transformation matrix of the positioning device a from the navigation coordinate system to the body coordinate system at time ki, The position of positioning device b in the navigation coordinate system of positioning device a is sent at time ki.

[0179] The calculation formula for the position of the sending positioning device b in the body coordinate system of the positioning device a estimated by inertial navigation is as follows:

[0180]

[0181] In the formula is the transformation matrix from the k-time coordinate system of the receiving positioning device a to the ki-time coordinate system, To send the change matrix of the coordinate system of positioning device b at time ki to the coordinate system at time k, is the relative position of the sending positioning device b in the coordinate system of the receiving positioning device a at time k, The position of the positioning device b in the coordinate system of the positioning device b is sent at time ki.

[0182] The aircraft pose estimated by inertial navigation is the relative pose in the navigation coordinate system, then The calculation of is as follows:

[0183]

[0184]

[0185]

[0186]

[0187] In the formula The pose transformation matrix from the northeast celestial coordinate system of positioning device b to the body coordinate system at time k. is the pose transformation matrix from the Earth-centered Earth-fixed rectangular coordinate system to the Northeast Sky coordinate system of the sending positioning device b at time k. The pose transformation matrix from the north-east coordinate system of the positioning device b to the Earth-centered Earth-fixed coordinate system is sent at time ki. is the pose transformation matrix from the machine system of the sending positioning device b to the north-east coordinate system of the sending positioning device b at time ki.

[0188] Step 6: Based on the time series, the relative position residuals of the previous m moments are combined to establish the relative pose optimization equation, and the Levenberg-Macart method is used to optimize the relative pose between positioning devices.

[0189] The specific process is:

[0190] The weighted sum of the relative position residuals established at the previous m moments of the current moment (the kth moment) is used to establish the relative pose estimation optimization equation f(x) based on the time series, as follows:

[0191]

[0192] Where W k-i is the weight of the ki-th relative position error, is the position residual established in step 5. The optimized variable x is the relative position of the body coordinate system of positioning device b in the body coordinate system of positioning device a at the current moment.

[0193] By solving the above optimization equations, the relative posture information between positioning devices can be obtained.

[0194] Based on the above method, the present invention also proposes a relative positioning system combining a data link and an IMU, comprising: an inertial navigation module, a data link measurement module, a flight state parameter and data link measurement time alignment module, and a relative posture estimation module; wherein:

[0195] The inertial navigation module calculates the flight posture based on the acceleration and angular velocity collected by the accelerometer and gyroscope and integrates it into the state parameter frame of the positioning device, and transmits the state parameter frame obtained at each moment to other drones through the data link.

[0196] The data link measurement module calculates the distance between the positioning devices based on the reception and release times of the data link information frames, using a motion model to eliminate erroneous measurements. Based on the angle and distance values at the time of data link reception, the distance is decomposed to determine the relative position of positioning device b in the north-east coordinate system of positioning device a.

[0197] The flight state parameter and data link measurement time alignment module performs linear interpolation on the inertial navigation estimated posture of positioning device b based on the data link timestamp to obtain the flight state parameters at the measurement time.

[0198] The relative pose estimation module converts the relative positions measured by the data link into the aircraft coordinate system and calculates the relative positions between the aircraft at each moment based on the first m state parameters. Using the relative positions between the aircraft measured by the data link and those calculated by inertial navigation, the relative position residuals between the aircraft are constructed, and an optimization equation for the relative pose between the UAVs based on a time series is established. The real-time relative pose between the UAVs is determined through iterative optimization using the Levenberg-Macart method.

[0199] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention.

Claims

1. A method for estimating relative pose between mobile devices using an IMU and a data link, characterized by: The mobile device is equipped with a positioning device, which is composed of a rigidly connected IMU device and a data link device. During the relative pose estimation process, the positioning devices can communicate in real time, exchanging data collected by their respective IMUs and their own calculated position and pose information. The relative pose estimation method comprises the following steps: Step 1: For each mobile device, use the IMU device installed on it to collect the acceleration and angular velocity values in real time. Use the inertial navigation calculation method to integrate the acceleration and angular velocity to obtain the position and attitude information of the mobile device itself in the navigation coordinate system; Step 2: Integrate the calculated position, velocity, and posture information of the mobile device into a state information frame of the mobile device at each moment and output it through the data link; Step 3: Establish a data link distance calculation model. Calculate the distance between the two mobile devices based on the time difference between sending and receiving data link information. Calculate the position of the sending device in the northeast celestial coordinate system of the receiving device using the azimuth and altitude angles of the data link device when receiving the information. Step 4: Based on step 1, the positioning device obtains its own position information through IMU calculation. The positioning device transmits the position information calculated by the IMU to other positioning devices through the data link. After receiving the information, the positioning device at the receiving end analyzes the information and obtains the relative distance measurement value between the two mobile devices based on the data link measurement according to the method in step 3. At the receiving end, the linear difference method is used based on the data link measurement time to align the time of its own posture information and the data link measurement information: Construct a data structure containing the data link measurement time, the relative distance measured by the data link, and the relative angle measured: the distance frame; construct a data structure containing the inertial navigation posture calculation time, the position and posture calculated by the inertial navigation: the inertial navigation posture frame; After receiving the data link information sent by mobile device b, the data link receiving end of mobile device a calculates the relative distance and angle measured by this data link, constructs it into a distance frame, and stores it in the distance frame queue; at the same time, it parses the inertial navigation information of mobile device b contained in the data link information, constructs an inertial navigation pose frame, and stores it in the inertial navigation pose frame queue; The timestamp of the second distance frame received is used as the initial timestamp of the system. D0 , the average of the timestamps of the two adjacent inertial navigation pose frames before and after the second range frame is taken as the initial time of the inertial navigation pose frame at the transmitter: The system timestamps of the subsequent k-th inertial navigation pose frame and n-th distance frame are: Where t Ik is the acquisition timestamp of the kth inertial navigation pose frame, t Dn is the acquisition timestamp of the nth range frame; Retrieve two consecutive inertial navigation pose frames that meet the conditions in the saved inertial navigation pose frame queue: In the formula The timestamps of the previous and next inertial navigation pose frames when collecting the nth range frame are respectively used. Linear interpolation is performed on the kth and k+1th inertial pose frames. The formula is as follows: Get P n ,θ n The position and attitude of the mobile device estimated by inertial navigation after time alignment; P k , P k+1 and θ k ,θ k+1 are the estimated position and attitude of the mobile device in the inertial navigation frames of the kth frame and the k+1th frame respectively; Step 5: After completing the alignment of the positioning information data, the posture information of the positioning device sending the information is sorted and its posture is converted to the body coordinate system of the receiving device. The relative position residual of the sending device in the body coordinate system of the receiving device is established by using the calculated relative position of the sending device in the body coordinate system of the receiving device and the relative position of the sending device in the body coordinate system of the receiving device measured by the data link; Step 6: Based on the time series, the relative position residuals of the previous m moments are combined to establish the relative pose optimization equation, and the Levenberg-Macart method is used to optimize the relative pose between positioning devices.

2. The method for estimating relative pose between mobile devices using an IMU and a data link combination according to claim 1, wherein: Calibrate the relative pose between the IMU and the data link: Use the IMU coordinate system as the positioning device coordinate system, and calibrate the relative pose transformation between the data link coordinate system and the IMU coordinate system through manual measurement.

3. The method for estimating relative pose between mobile devices using an IMU and a data link combination according to claim 1, wherein: Calibrate the data link measurement delay: The calculation formula for the measured distance between a pair of data links is: In the formula The moment when data link a on a mobile device sends a signal, data link b on another mobile device receives the signal sent by data link a and replies to data link a. The moment when data link a receives the reply signal is c is the speed of light, δt is the delay in sending, receiving and processing the data link; Through n ranging results and the GNSS information at the corresponding time, δt is obtained by solving the following formula: d e =||T el P a -T el P b || Where d m , d e are the measured and calculated values of the distance, and are the measured and calculated values of the distance at time i; T el is the transformation matrix from the latitude and longitude coordinate system to the Earth-centered Earth-fixed rectangular coordinate system, P a , P b They are the position information of the positioning device that sends data link information and the positioning device that receives data link information in the longitude and latitude coordinate system.

4. The method for estimating relative pose between mobile devices using an IMU and a data link combination according to claim 1, wherein: In step 1, the inertial navigation calculation method is as follows: Step 1.1: Using the attitude angle measured by the gyroscope at time k, the attitude transformation matrix from the mobile device's body coordinate system to the navigation coordinate system at time k is obtained through the following process: First, solve the attitude transformation matrix from the current body coordinate system to the navigation coordinate system, and obtain the calculation method of the current mobile device posture as follows: Where: is the attitude transformation matrix from the body coordinate system to the navigation coordinate system at time k, n T k,k-1 is the transformation matrix of the navigation coordinate system from time k-1 to time k, and this transformation matrix is the unit matrix; is the transformation matrix from the body coordinate system to the navigation coordinate system at time k-1; is the posture change matrix of the body coordinate system from time k-1 to time k, and is solved by integrating the angular velocity measured by the gyroscope from time k-1 to time k; ω(τ) is the angular velocity measured by the gyroscope at time τ, Δθ k is the IMU posture rotation from k-1 to k, φ k is the attitude of the IMU in the navigation coordinate system at time k; Step 1.2: The acceleration a measured by the accelerometer in the body coordinate system at time k is b , through the transformation matrix from the body coordinate system to the navigation coordinate system at time k Convert to the navigation coordinate system: Step 1.3: Using the acceleration value in the navigation coordinate system obtained in step 1.2, use the velocity differential equation Calculate the speed of the mobile device, where is the differential value of the inertial navigation velocity, f n is the acceleration measured in the navigation coordinate system, is the Coriolis acceleration, where V n is the speed of the mobile device in the navigation coordinate system, is the angular velocity of the Earth in the navigation coordinate system; is the centripetal acceleration caused by the carrier motion, where is the angular velocity of the mobile device around the earth in the navigation coordinate system; g n is the gravitational acceleration in the navigation coordinate system; for the incremental information sampling in each update cycle, the speed of the mobile device at time k is: In the formula is the speed of the mobile device in the navigation coordinate system at time k, k-1, is the pose transformation matrix from the machine system to the navigation coordinate system at time k, is the pose transformation matrix of the body coordinate system from time t to time k; Step 1.4: Using the velocity of the mobile device in the navigation coordinate system obtained in step 1.3, the trapezoidal integration method can be used to implement the update equation of the mobile device position: Among them, P k-1 With P k are the positions of the mobile device in the navigation coordinate system at time k-1 and time k, including the latitude L, longitude λ, and altitude h of the mobile device; τ is the time interval between time k-1 and time k; M pv(k-1 / 2) The matrix is the transformation matrix from navigation coordinates at time k-1 / 2 to longitude and latitude coordinates; R M and R N They respectively represent the principal curvature radii of the meridian and the meridian at the location of the mobile device.

5. The method for estimating relative pose between mobile devices using an IMU and a data link combination according to claim 1, wherein: In step 3, bad distance measurements are filtered out by the following process: By calculating the relative distance measurement values at consecutive moments and comparing the distance measurement value changes between adjacent moments, bad measurement points can be eliminated: δd k-1 =d k-1 -d k-2 δd k =d k -d k-1 Where d k-2 ,d k-1 ,d k are the relative distance measurements at the k-2, k-1, and k moments respectively; δd k-1 is the difference between the relative distance measurements at time k-2 and time k-1, δd k is the difference between the relative distance measurement values at the kth moment and the k-1th moment, Δd is the relative distance change threshold, and the response time is δd. k-1 =0.

6. The method for estimating relative pose between mobile devices using an IMU and a data link combination according to claim 1, wherein: In step 5, the coordinate system is transformed to establish the relative position residual. The specific process is as follows: Step 5.1: Save the latest m distance frames received and the corresponding interpolated inertial pose frames; Step 5.2: Create position residuals: In the formula The position of the sending positioning device b in the body coordinate system of the receiving positioning device a corresponding to the state parameter of the ki-th frame measured by the data link; The position of the sending positioning device b in the body coordinate system of the receiving positioning device a obtained by inertial navigation estimation; The data link measures the position of the transmitting positioning device b in the northeastern sky coordinate system at time ki of the receiving positioning device a, and converts it into the position of the transmitting positioning device b in the body coordinate system of the receiving positioning device a. The calculation formula is as follows: In the formula To receive the pose transformation matrix of the positioning device a from the navigation coordinate system to the body coordinate system at time ki, Send the position of positioning device b in the navigation coordinate system of positioning device a at time ki; The calculation formula for the position of the sending positioning device b in the body coordinate system of the positioning device a estimated by inertial navigation is as follows: In the formula is the transformation matrix from the k-time coordinate system of the receiving positioning device a to the ki-time coordinate system, To send the change matrix of the coordinate system of positioning device b at time ki to the coordinate system at time k, is the relative position of the sending positioning device b in the coordinate system of the receiving positioning device a at time k, The position of the positioning device b in the coordinate system of the positioning device b is sent at time ki; The pose of the mobile device estimated using inertial navigation is the relative pose in the navigation coordinate system. The calculation of is as follows: In the formula The pose transformation matrix from the northeast celestial coordinate system of positioning device b to the body coordinate system is sent at time k; is the pose transformation matrix from the Earth-centered Earth-fixed rectangular coordinate system to the Northeast Sky coordinate system of the sending positioning device b at time k; The pose transformation matrix from the north-east coordinate system of positioning device b to the Earth-centered Earth-fixed coordinate system is sent at time ki; is the pose transformation matrix from the machine system of the sending positioning device b to the north-east coordinate system of the sending positioning device b at time ki.

7. The method for estimating relative pose between mobile devices using an IMU and a data link combination according to claim 1, wherein: The specific process of step 6 is: The weighted sum of the relative position residuals established at the m moments before the current k-th moment is used to establish the relative pose estimation optimization equation f(x) based on the time series, as follows: Where W k-i is the weight of the ki-th relative position error, is the position residual established in step 5; the optimized variable x is the relative position of the body coordinate system of positioning device b in the body coordinate system of positioning device a at the current moment.

8. A relative positioning system between mobile devices using an IMU combined with a data link according to any one of claims 1 to 7, characterized in that: include: Inertial navigation module, data link measurement module, flight status parameter and data link measurement time alignment module, relative posture estimation module; among which: The inertial navigation module calculates the flight posture based on the acceleration and angular velocity collected by the accelerometer and gyroscope and integrates it into the state parameter frame of the positioning device. The state parameter frame obtained at each moment is transmitted to other mobile devices through the data link; The data link measurement module calculates the distance between the positioning devices based on the reception and release time of the data link information frame, and uses the motion model to eliminate erroneous measurement values. Based on the angle and distance values at the time of data link reception, the distance is decomposed to obtain the relative position of positioning device b in the north-east coordinate system of positioning device a. The flight state parameter and data link measurement time alignment module performs linear interpolation on the inertial navigation estimated pose of positioning device b based on the data link timestamp to obtain the flight state parameters at the measurement time; The relative pose estimation module converts the relative position measured by the data link into the body coordinate system, and calculates the relative position between the mobile devices at each moment based on the first m state parameters; uses the relative position between the mobile devices measured by the data link and the relative position calculated by inertial navigation to construct the relative position residual between the mobile devices, and establishes the relative pose optimization equation between the mobile devices based on the time series; uses the Levenberg-Macart method to iteratively optimize and obtain the real-time relative pose between the mobile devices.

Images