Mobile base station system based on satellite navigation and data fusion method

Abstract

The invention discloses a satellite navigation-based mobile base station system and a data fusion method, and the system comprises a positioning module, a calculation module, an IMU module, a magnetometer, a barometer and an external interface, can provide reliable software and hardware support for the data fusion method, has abundant external output interfaces, and meets most communication interface deployment requirements. The data fusion method based on the system comprises attitude fusion and position fusion, can solve pose information in real time, can improve the output frequency and precision of data such as position, and meets the requirement of high update rate of data such as mobile base station positioning in high-speed movement of the unmanned aerial vehicle. Meanwhile, the data fusion method based on the system can eliminate the armed lever effect caused by inconsistency of the antenna installation position and the host, thereby further improving the accuracy of position and speed data.

Description

Technical Field

[0001] The present invention relates to a data fusion method, in particular to a satellite-based mobile base station system and a data fusion method, and belongs to the technical field related to unmanned aerial vehicle positioning and navigation. Background Art

[0002] With the rapid development of science and technology, drones are widely used in military and civilian fields. In order to facilitate deployment and withdrawal, the recovery of drones on mobile platforms has received widespread attention. In order to achieve take-off and landing in the narrow space of the mobile platform, high requirements are placed on the navigation and positioning of drones. At present, using mobile base station RTK to obtain the relative position with the mobile platform is a common method, but the current mobile base station has the following limitations: there are fewer types of peripheral interfaces and the scalability of the communication interface is poor; the positioning data output by the mobile base station is updated at a low frequency, which cannot meet the demand for the update rate of the base station positioning data when the drone moves rapidly, which will cause a lag in the control of the drone; due to the serious vibration of the mobile platform during movement, the lack of effective filtering means leads to jitter problems in the output positioning data; due to the limited layout on the mobile platform, in most cases, the installation positions of the mobile base station host and the positioning receiving antenna are inconsistent. When the mobile platform rotates, a boom effect will occur, resulting in positioning deviation.

[0003] Based on the above problems, it is necessary to improve the mobile base station system, eliminate deviations, meet the needs of high-speed drones for base station positioning data, and improve the accuracy of position and speed data. Summary of the invention

[0004] The purpose of the present invention is to address the above-mentioned problems existing in the prior art and to propose a mobile base station system and a data fusion method suitable for satellite navigation of unmanned aerial vehicles to solve the above-mentioned problems existing in the prior art.

[0005] In order to achieve the above object, the present invention adopts the following technical scheme:

[0006] The present invention first discloses a mobile base station system based on satellite navigation, comprising:

[0007] Positioning module: It consists of satellite receiving antenna 1, satellite receiving antenna 2 and positioning and directional board. The positioning and directional board is connected to satellite receiving antenna 1 and satellite receiving antenna 2 through radio frequency cables, and outputs position, speed and heading angle information to the computing module (MCU).

[0008] IMU module: It consists of an accelerometer and a gyroscope, and is used to collect acceleration and angular velocity flight data;

[0009] Magnetometer: consists of a three-axis magnetic sensor;

[0010] Barometer: It consists of a static pressure sensor and is used to collect temperature and air pressure values;

[0011] It also includes external interfaces, consisting of CAN, serial port, network port, Bluetooth, and WIFI, which are used to output the position, speed, and attitude data after data fusion.

[0012] Among them, the positioning module outputs the position, speed, direction angle and RTCM data stream calculated by the positioning board to the MCU through the serial port; the IMU module outputs the measured three-axis angular rate and three-axis acceleration to the MCU through SPI; the magnetometer outputs the measured three-axis magnetic induction data to the MCU through SPI; the barometer outputs the measured temperature and air pressure data to the MCU through II2C.

[0013] The present invention also discloses a data fusion method implemented based on the above system, including attitude fusion algorithm and position fusion algorithm. The attitude fusion algorithm is to perform attitude fusion algorithm solution on the acceleration and angular velocity collected by the IMU module, the magnetic field data measured by the magnetometer, and the heading data collected in the positioning module. The position fusion algorithm is to perform position fusion algorithm solution on the acceleration collected by the IMU module, the position and velocity collected by the positioning module, and the altitude data collected by the barometer.

[0014] Among them, the specific steps of the posture fusion algorithm include:

[0015] (1) Through the attitude filter in the IMU module, the prediction and update formulas are iterated to output the pitch angle θ, heading angle ψ0, and roll angle φ;

[0016] The attitude filter adopts the extended Kalman filter algorithm, and the state variable X = [q0q1 q2 q3b x b y b z ], where q0, q1, q2, q3 are the unitized four elements, b x 、b y 、b z They are x-axis angular velocity bias, y-axis angular velocity bias and z-axis angular velocity bias respectively; measured variable Z = [a ux a uy a uz ], where a ux 、a uy 、a uz They are the unitized x-axis body acceleration, the unitized y-axis body acceleration and the unitized z-axis body acceleration, respectively. According to the prediction and update formula of the extended Kalman filter, the pitch angle θ, the heading angle ψ0 and the roll angle φ are iterated.

[0017] (2) Using heading sub-filter 1 and heading sub-filter 2 to calculate heading angle ψ1 and heading angle ψ2 respectively;

[0018] First, the calculation formula of the observed variable of heading sub-filter 1 is as follows:

[0019]

[0020] Among them, mag ex 、mag ey They are respectively the eastward magnetic field intensity component and the northward magnetic field intensity component in the geographic coordinate system, and the calculation formula is as follows:

[0021]

[0022] mag ubx 、mag uby are the unitized x-axis magnetic field intensity component and the unitized y-axis magnetic field intensity component in the carrier coordinate system measured by the magnetometer. According to the Kalman filter prediction and update formula, the heading angle ψ1 is calculated iteratively;

[0023] Next, the observed variable of heading subfilter 2 is the heading angle measured by the dual antennas of the positioning module, and the heading angle ψ2 is calculated by iteration according to the Kalman filter prediction and update formula;

[0024] (3) Select and process the heading angle

[0025] The main filter selects and processes the heading angle according to the status of heading sub-filter 1 and heading sub-filter 2. When the status of the two sub-filters are normal, the output heading deviation ψ'=ψ2-ψ1 is calculated. The output formula of the main filter heading is as follows:

[0026]

[0027] Among them, state1 and state2 represent the state of heading sub-filter 1 and heading sub-filter 2 respectively, with a value of 1 for normal and 0 for abnormal. When the state of heading sub-filter 2 is normal, ψ2 is output; when the state of heading sub-filter 2 is abnormal but the state of heading sub-filter 1 is normal, ψ1+ψ' is output; when the state of heading sub-filter 2 and the state of heading sub-filter 1 are both abnormal, ψ0 is output.

[0028] The steps of the position fusion algorithm are as follows:

[0029] (1) Parameter initialization

[0030] Initial parameter settings are performed for the covariance matrix P, process noise matrix Q, measurement noise matrix R and state variable matrix X, where the state variable X = [pos N POS E POS D vel N vel Evel D acc bN acc bE acc bD ], including the north distance pos N , east distance pos E , vertical distance pos D , north speed vel N , eastward speed vel E , vertical speed vel D , north acceleration zero bias acc bN , Eastward acceleration zero bias acc bE , vertical acceleration zero bias acc bD ; The covariance matrix P is 9×9 in size; the process noise matrix Q is 9×1 in size; and the measurement noise matrix R is 3×1 in size.

[0031] (2) Prediction Module

[0032] According to the differential equation of state variables The method for obtaining the differential equation of the state quantity X is as follows:

[0033]

[0034] Where f1(X), f2(X)…f9(X) are differential equations of the state variables, where the north acceleration zero bias acc bN 、East acceleration zero bias acc bE and vertical acceleration zero bias acc bD Through the carrier forward acceleration zero bias acc bX 、Carrier right acceleration zero deviation acc bY and the carrier vertical acceleration zero bias acc bZ Coordinate system conversion acquisition:

[0035]

[0036] Among them, θ, ψ, and φ are the pitch angle, heading angle, and roll angle respectively;

[0037] (3) Matrix update

[0038] 1) State transfer matrix update: The state transfer matrix Φ = I + F * △t, where I is a unit diagonal matrix of size 9 × 9, and F is the Jacobian matrix of f(X);

[0039] 2) Covariance matrix update: covariance matrix P k =φP k-T φ T +Q, where P k-T is the covariance matrix of the solution at the previous moment, φ Tis the transposed matrix of the state transfer matrix.

[0040] (4)Amendment

[0041] Observed variable Z = [pos N POS E POS D vel N vel E vel D ], where Z is the position and speed data measured in real time by the positioning module, where pos N ,pos E ,pos D are the north distance, east distance and vertical distance respectively; vel N ,vel E ,vel D are the north velocity, east velocity and vertical velocity respectively;

[0042] 1) Arm effect compensation

[0043] There is a positional deviation between the satellite receiving antenna and the base station host in the positioning module during installation and deployment. When the deployment carrier rotates, the position and speed information measured at the installation point of the positioning module and the base station host will differ, resulting in a lever effect. N ,pos E ,pos D ,vel N ,vel E ,vel D The data is after the arm effect compensation. The position information compensation formula is as follows:

[0044]

[0045] Where offset bX 、offset bY 、offset bZ They are the deviation values ​​of the installation position of the satellite receiving antenna 1 (main antenna) from the center of the base station host in the carrier coordinate system; pos offsetN ,pos offsetE ,pos offsetD are the position compensation values ​​of the three axes in the geographic coordinate system, and θ, ψ, and φ are the pitch angle, heading angle, and roll angle respectively.

[0046] The formula for speed information compensation is as follows:

[0047]

[0048] Among them, w x 、w y 、w zThey are the three-axis angular velocity measured by the IMU module, vel offsetX ,vel offsetY ,vel offsetZ They are the velocity compensation values ​​of the three axes in the carrier coordinate system, vel offsetN ,vel offsetE ,vel offsetD They are the speed compensation values ​​of the three axes in the geographic coordinate system.

[0049] The position and speed data after arm effect compensation are obtained, and the formula is as follows:

[0050]

[0051] 2) Deviation Matrix Update

[0052] The deviation matrix E=Z-HX calculates the deviation between the measured value and the predicted value, where the measurement matrix H is the Jacobian matrix of the observed variable Z with respect to the state variable X, and HX is the product of the measurement matrix H and the state variable matrix X.

[0053] (5) Residual Detection

[0054] The residual calculation formula is as follows:

[0055]

[0056] Where E T is the transposed matrix of the deviation matrix, H T is the transposed matrix of the measurement matrix, R is the measurement noise matrix, HPH T is the product of the measurement matrix, the covariance matrix, and the measurement transposed matrix.

[0057] If the residual calculation value is greater than the set value, the iteration of the optimal filtering module and the optimal filtering recursive module will not be performed. When the error accumulation timeout, the fault will be reported. It can be used to identify satellite signal interference, such as unstable or interfered satellite signals, measurement data jumps, etc.

[0058] (6) Optimal filter gain module

[0059] Gain Matrix Among them, H T is the transposed matrix of the measurement matrix, R is the measurement noise matrix, PH is the product of the covariance matrix and the measurement matrix, HPH T is the product of the measurement matrix, the covariance matrix, and the measurement transposed matrix.

[0060] (7) Optimal filter gain module

[0061] 1) Covariance matrix update

[0062] Covariance update matrix Pk =P k-T -KHP k-T , where P k-T is the covariance matrix of the solution at the previous moment, KHP k-T It is the product of the gain matrix, the measurement matrix and the covariance matrix at the previous moment.

[0063] 2) State matrix update

[0064] State update matrix X k =X k-T +K(Z-HX k-T ), where X k-T HX is the state variable updated at the last moment. k-T is the product of the measurement matrix and the state variable of the previous moment.

[0065] Since it takes time for the acceleration bias to converge, especially when the attitude changes quickly, it will cause the error of the acceleration bias in the geographic coordinate system. Therefore, the corrected acceleration bias is converted to the acceleration bias in the carrier coordinate system. The formula is as follows:

[0066]

[0067] The present invention is beneficial in that:

[0068] (1) The satellite navigation-based mobile base station system of the present invention has strong scalability and is equipped with a variety of communication interfaces, which can meet most deployment communication requirements.

[0069] (2) By using enhanced Kalman filtering to fuse the acceleration data in the IMU and the position and velocity data in the positioning board, the output frequency and accuracy of positioning data can be improved to meet the needs of high-speed UAVs for base station positioning data. During the fusion process, since it takes time for the acceleration zero bias to converge, especially when the attitude changes quickly, it will cause the error of the acceleration zero bias in the geographic coordinate system. Therefore, the corrected acceleration zero bias is converted into the acceleration zero bias in the carrier coordinate system, thereby improving the calculation efficiency.

[0070] (3) In the present invention, when performing the position fusion algorithm, the acceleration zero bias coordinate conversion is used to solve the problem that the acceleration zero bias in the geographic coordinate system needs to be re-converged due to the change in the carrier's posture, which can improve the accuracy of the position and velocity data and meet the needs of high dynamic movement of the carrier; the posture data calculated by the posture fusion algorithm, combined with the angular velocity data, can eliminate the arm effect caused by the inconsistent installation position of the host and the positioning antenna, thereby improving the accuracy of the position and velocity data. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 It is an electrical connection diagram of the mobile base station system of the present invention;

[0072] Figure 2 It is a block diagram of the posture fusion algorithm of the present invention;

[0073] Figure 3 This is a block diagram of the position fusion algorithm of the present invention. DETAILED DESCRIPTION

[0074] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0075] Example 1

[0076] See also Figure 1 This embodiment discloses a mobile base station system based on satellite navigation, including: a positioning module, a computing module, an IMU module, a magnetometer, a barometer and an external interface; the positioning module, the IMU module, the magnetometer and the barometer are all connected to the computing module to realize data transmission and feedback. Specifically, the positioning module, the IMU module, the magnetometer and the barometer are connected to the computing module through a serial port, SPI, SPI and II2C interface respectively.

[0077] The positioning module consists of satellite receiving antenna 1, satellite receiving antenna 2 and positioning and directional board. The positioning and directional board is connected to satellite receiving antenna 1 and satellite receiving antenna 2 through RF cables, outputs information such as position, speed and heading angle to the calculation module, and can output RTCM data to the outside. The IMU module consists of an accelerometer and a gyroscope, which can be used to collect flight data such as acceleration and angular velocity. The external interface consists of CAN, serial port, network port, Bluetooth, and WIFI, and can be connected to other terminals through the external interface.

[0078] The calculation module performs digital filtering on the received data, specifically including: performing attitude fusion algorithm solution on the acceleration and angular velocity collected by the IMU module, the magnetic field data measured by the magnetometer and the heading data collected by the positioning module; performing position fusion algorithm solution on the acceleration collected by the IMU module, the position and speed collected by the positioning module and the altitude data collected by the barometer, and outputting the fused position, speed, attitude and other data through the external interface.

[0079] Example 2

[0080] See also Figure 2 and Figure 3 The present invention discloses a data fusion method for a mobile base station system based on satellite navigation, which specifically includes attitude fusion and position fusion. The specific steps are as follows:

[0081] (I) Posture Fusion

[0082] (1) Through the attitude filter in the IMU module, the prediction and update formulas are iterated to output the pitch angle θ, heading angle ψ0, and roll angle φ.

[0083] The attitude filter adopts the extended Kalman filter algorithm, and the state variable X = [q0q1 q2 q3b x b y b z ], where q0, q1, q2, q3 are the unitized four elements, b x 、b y 、b z They are x-axis angular velocity bias, y-axis angular velocity bias and z-axis angular velocity bias respectively; measured variable Z = [a ux a uy a uz ], where a ux 、a uy 、a uz They are the unitized x-axis body acceleration, the unitized y-axis body acceleration and the unitized z-axis body acceleration respectively. According to the prediction and update formula of the extended Kalman filter, they are iterated to output the pitch angle θ, the heading angle ψ0 and the roll angle φ.

[0084] (2) Heading sub-filter 1 and heading sub-filter 2 are used to calculate the heading angle ψ1 and the heading angle ψ2 respectively, and the heading angle is selected and processed.

[0085] The Kalman filter algorithm is used, and the state variable is the heading angle ψ0 output by the attitude filter.

[0086] First, the calculation formula of the observed variable of heading sub-filter 1 is as follows:

[0087]

[0088] Among them, mag ex 、mag ey They are respectively the eastward magnetic field intensity component and the northward magnetic field intensity component in the geographic coordinate system, and the calculation formula is as follows:

[0089]

[0090] mag ubx 、mag uby They are the unitized x-axis magnetic field intensity component and the unitized y-axis magnetic field intensity component in the carrier coordinate system measured by the magnetometer. The heading angle ψ1 is calculated by iterating according to the Kalman filter prediction and update formula.

[0091] Next, the observed variable of the heading subfilter 2 is the heading angle measured by the dual antennas of the positioning module. The heading angle ψ2 is calculated by iteration according to the Kalman filter prediction and update formula.

[0092] Finally, the main filter selects and processes the heading angle according to the status of heading sub-filter 1 and heading sub-filter 2. When the status of the two sub-filters are normal, the output heading deviation ψ'=ψ2-ψ1 is calculated. The output formula of the main filter heading is as follows:

[0093]

[0094] Among them, state1 and state2 represent the state of heading sub-filter 1 and heading sub-filter 2 respectively, with a value of 1 for normal and 0 for abnormal. When the state of heading sub-filter 2 is normal, ψ2 is output; when the state of heading sub-filter 2 is abnormal but the state of heading sub-filter 1 is normal, ψ1+ψ' is output; when the state of heading sub-filter 2 and the state of heading sub-filter 1 are both abnormal, ψ0 is output.

[0095] (II) Position Fusion

[0096] (1) Parameter initialization

[0097] Initial parameter settings are performed for the covariance matrix P, process noise matrix Q, measurement noise matrix R and state quantity matrix X, where the state quantity matrix X = [pos N POS E POS D vel N vel E vel D acc bN acc bE acc bD ], including the north distance pos N , east distance pos E , vertical distance pos D , north speed vel N , eastward speed vel E , vertical speed vel D , north acceleration zero bias acc bN , Eastward acceleration zero bias acc bE , vertical acceleration zero bias acc bD ; The size of the covariance matrix P is 9×9; the size of the process noise matrix Q is 9×1; the size of the measurement noise matrix R is 3×1.

[0098] (2) Prediction Module

[0099] According to the differential equation of state variables Find the differential equation of the state quantity X:

[0100]

[0101] Among them, f1(X), f2(X)...f9(X) are the differential equations of each state variable, where the north acceleration zero bias acc bN 、East acceleration zero bias acc bE and vertical acceleration zero bias acc bD Respectively through the carrier forward acceleration zero bias acc bX 、Carrier right acceleration zero deviation acc bY and the carrier vertical acceleration zero bias acc bZ Coordinate system conversion acquisition:

[0102]

[0103] θ, ψ, φ are pitch angle, heading angle and roll angle respectively;

[0104] (4) Matrix update

[0105] State transfer matrix update: state transfer matrix Φ = I + F * △ t, where I is a unit diagonal matrix of size 9 × 9, and F is the Jacobian matrix of f(X).

[0106] Covariance matrix update: covariance matrix P k =φP k-T φ T +Q, where P k-T is the covariance matrix of the solution at the previous moment, φ T is the transposed matrix of the state transfer matrix.

[0107] (4)Amendment

[0108] Correction of the correlation operation is the core innovation of the present invention and plays a crucial role in the precision and accuracy of data fusion.

[0109] Observed variable Z = [pos N POS E POS D vel N vel E vel D ], Z is the position and speed data measured in real time by the positioning module, where pos N ,pos E ,pos D are the north distance, east distance and vertical distance respectively; vel N ,vel E ,vel D They are north velocity, east velocity and vertical velocity respectively. Specifically, the correction includes the following two aspects:

[0110] 3) Boom effect compensation

[0111] Since there is a positional deviation between the satellite receiving antenna and the base station host in the positioning module during installation and deployment, when the deployment carrier rotates, the position and speed information measured at the installation point of the positioning module and the base station host will differ, resulting in a lever effect. N ,pos E ,pos D ,vel N ,vel E ,vel D The data is after the arm effect compensation. The position information compensation formula is as follows:

[0112]

[0113] Among them, offset bX 、offset bY 、offset bZ They are the deviation values ​​of the installation position of the satellite receiving antenna 1 (main antenna) from the center of the base station host in the carrier coordinate system; pos offsetN ,pos offsetE ,pos offsetD are the position compensation values ​​of the three axes in the geographic coordinate system, and θ, ψ, and φ are the pitch angle, heading angle, and roll angle respectively.

[0114] The formula for speed information compensation is as follows:

[0115]

[0116] Among them, w x 、w y 、w z They are the three-axis angular velocity measured by the IMU module, vel offsetX ,vel offsetY ,vel offsetZ They are the velocity compensation values ​​of the three axes in the carrier coordinate system, vel offsetN ,vel offsetE ,vel offsetD They are the speed compensation values ​​of the three axes in the geographic coordinate system.

[0117] The position and speed data after arm effect compensation are obtained, and the formula is as follows:

[0118]

[0119] 4) Deviation matrix update

[0120] The deviation matrix E=Z-HX calculates the deviation between the measured value and the predicted value, where the measurement matrix H is the Jacobian matrix of the observed variable Z with respect to the state variable X, and HX is the product of the measurement matrix H and the state variable matrix X.

[0121] (5) Residual Detection

[0122] The residual calculation formula is as follows:

[0123]

[0124] Where E T is the transposed matrix of the deviation matrix, H T is the transposed matrix of the measurement matrix, R is the measurement noise matrix, HPH T is the product of the measurement matrix, the covariance matrix, and the measurement transposed matrix.

[0125] If the residual calculation value is greater than the set value, the iteration of the optimal filtering module and the optimal filtering recursive module will not be performed. When the error accumulation timeout, the fault will be reported. It can be used to identify satellite signal interference. If the satellite signal is unstable or interfered, the measurement data will jump.

[0126] (7) Optimal filter gain module

[0127] Gain Matrix Among them, H T is the transposed matrix of the measurement matrix, R is the measurement noise matrix, PH is the product of the covariance matrix and the measurement matrix, HPH T is the product of the measurement matrix, the covariance matrix, and the measurement transposed matrix.

[0128] 3) Covariance matrix update

[0129] Covariance update matrix P k =P k-T -KHP k-T , where P k-T is the covariance matrix of the solution at the previous moment, KHP k-T It is the product of the gain matrix, the measurement matrix and the covariance matrix at the previous moment.

[0130] 4) State matrix update

[0131] State update matrix X k =X k-T +K(Z-HX k-T ), where X k-T HX is the state variable updated at the last moment. k-T is the product of the measurement matrix and the state variable of the previous moment.

[0132] Since it takes time for the acceleration bias to converge, especially when the attitude changes rapidly, it will cause the error of the acceleration bias in the geographic coordinate system. Therefore, the corrected acceleration bias is converted to the acceleration bias in the carrier coordinate system. The formula is as follows:

[0133]

[0134] In summary, the satellite-guided mobile base station system of the present invention can provide reliable software and hardware support for the data fusion method. The system has a rich external output interface and meets the requirements of most deployed communication interfaces. The data fusion method based on the system includes attitude fusion and position fusion, which can solve the attitude information in real time, improve the frequency and accuracy of position and other data output, and meet the high-speed movement of drones for high update rates of mobile base station positioning and other data. At the same time, the data fusion method based on the system can eliminate the boom effect caused by the inconsistency between the antenna installation position and the host, thereby further improving the accuracy of the position and speed data.

[0135] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any form, and any technical solution obtained by equivalent replacement or equivalent transformation falls within the protection scope of the present invention.

Claims

1. A mobile base station system based on satellite navigation, characterized in that: include: Positioning module: It consists of satellite receiving antenna 1, satellite receiving antenna 2 and positioning and directional board. The positioning and directional board is connected to satellite receiving antenna 1 and satellite receiving antenna 2 through radio frequency cables, and outputs position, speed and heading angle information to the calculation module. IMU module: It consists of an accelerometer and a gyroscope, and is used to collect acceleration and angular velocity flight data; Magnetometer: consists of a three-axis magnetic sensor; Barometer: It consists of a static pressure sensor and is used to collect temperature and air pressure values; It also includes external interfaces; The positioning module, IMU module, magnetometer and barometer are all connected to the computing module to realize data transmission and feedback.

2. The satellite navigation-based mobile base station system according to claim 1, characterized in that: The positioning module, IMU module, magnetometer and barometer are connected to the computing module via serial port, SPI, SPI and II2C interface respectively.

3. The satellite navigation-based mobile base station system according to claim 1, characterized in that: The external interface consists of CAN, serial port, network port, Bluetooth, and WIFI, and is used to output the position, speed, and posture data after data fusion.

4. The data fusion method of the satellite-guided mobile base station system according to claim 1 is characterized in that: At least: (1) Attitude fusion algorithm: The acceleration and angular velocity collected by the IMU module, the magnetic field data measured by the magnetometer, and the heading data collected by the positioning module are solved by the attitude fusion algorithm; (2) Position fusion algorithm: The position fusion algorithm is used to solve the acceleration data collected by the IMU module, the position and speed data collected by the positioning module, and the altitude data collected by the barometer.

5. The data fusion method of the mobile base station system based on satellite navigation according to claim 4 is characterized in that: The steps of the posture fusion algorithm are as follows: (1) Through the attitude filter in the IMU module, the prediction and update formulas are iterated to output the pitch angle θ, heading angle ψ0, and roll angle φ; The attitude filter adopts the extended Kalman filter algorithm, and the state variable X = [q0q1 q2 q3b x b y b z ], where q0, q1, q2, q3 are the unitized four elements, b x 、b y 、b z They are x-axis angular velocity bias, y-axis angular velocity bias and z-axis angular velocity bias respectively; measured variable Z = [a ux a uy a uz ], where a ux 、a uy 、a uz They are the unitized x-axis body acceleration, the unitized y-axis body acceleration and the unitized z-axis body acceleration, respectively. According to the prediction and update formula of the extended Kalman filter, the pitch angle θ, the heading angle ψ0 and the roll angle φ are iterated. (2) Using heading sub-filter 1 and heading sub-filter 2 to calculate heading angle ψ1 and heading angle ψ2 respectively; First, the calculation formula of the observed variable of heading sub-filter 1 is as follows: Among them, mag ex 、mag ey They are respectively the eastward magnetic field intensity component and the northward magnetic field intensity component in the geographic coordinate system, and the calculation formula is as follows: mag ubx 、mag uby are the unitized x-axis magnetic field intensity component and the unitized y-axis magnetic field intensity component in the carrier coordinate system measured by the magnetometer. According to the Kalman filter prediction and update formula, the heading angle ψ1 is calculated iteratively; Next, the observed variable of heading subfilter 2 is the heading angle measured by the dual antennas of the positioning module, and the heading angle ψ2 is calculated by iteration according to the Kalman filter prediction and update formula; (3) Select and process the heading angle The main filter selects and processes the heading angle according to the status of heading sub-filter 1 and heading sub-filter 2. When the status of the two sub-filters are normal, the output heading deviation ψ'=ψ2-ψ1 is calculated. The output formula of the main filter heading is as follows: Among them, state1 and state2 represent the state of heading sub-filter 1 and heading sub-filter 2 respectively, with a value of 1 for normal and 0 for abnormal. When the state of heading sub-filter 2 is normal, ψ2 is output; when the state of heading sub-filter 2 is abnormal but the state of heading sub-filter 1 is normal, ψ1+ψ' is output; when the state of heading sub-filter 2 and the state of heading sub-filter 1 are both abnormal, ψ0 is output.

6. The data fusion method of the mobile base station system based on satellite navigation according to claim 4 is characterized in that: The steps of the position fusion algorithm are as follows: (1) Parameter initialization Initial parameter settings are performed for the covariance matrix P, process noise matrix Q, measurement noise matrix R and state quantity matrix X, where the state quantity matrix X = [pos N POS E POS D vel N vel E vel D acc bN acc bE acc bD ], including the north distance pos N , east distance pos E , vertical distance pos D , north speed vel N , eastward speed vel E , vertical speed vel D , north acceleration zero bias acc bN , Eastward acceleration zero bias acc bE , vertical acceleration zero bias acc bD ; The size of the covariance matrix P is 9×9; the size of the process noise matrix Q is 9×1; the size of the measurement noise matrix R is 3×1; (2) Prediction Module According to the differential equation of state variables Find the differential equation of the state quantity X: Among them, f1(X), f2(X)...f9(X) are the differential equations of each state variable, where the north acceleration zero bias acc bN 、East acceleration zero bias acc bE and vertical acceleration zero bias acc bD Respectively through the carrier forward acceleration zero bias acc bX 、Carrier right acceleration zero deviation acc bY and the carrier vertical acceleration zero bias acc bZ Coordinate system conversion acquisition: θ, ψ, φ are pitch angle, heading angle and roll angle respectively; (3) Matrix update State transfer matrix update: state transfer matrix Φ = I + F * △t, where I is a unit diagonal matrix of size 9 × 9, and F is the Jacobian matrix of f(X); Covariance matrix update: covariance matrix P k =φP k-T φ T +Q, where P k-T is the covariance matrix of the solution at the previous moment, φ T is the transposed matrix of the state transfer matrix; (4)Amendment Perform arm effect compensation and deviation matrix update; (5) Residual Detection The residual calculation formula is as follows: Where E T is the transposed matrix of the deviation matrix, H T is the transposed matrix of the measurement matrix, R is the measurement noise matrix, HPH T is the product of the measurement matrix, the covariance matrix and the measurement transposed matrix; If the residual calculation value is greater than the set value, the iteration of the optimal filtering module and the optimal filtering recursive module will not be performed. When the error accumulation timeout, the fault will be reported. It can be used to identify the interference of satellite signals. If the satellite signal is unstable or interfered, the measurement data will jump, etc. (6) Optimal filter gain module Gain Matrix Among them, H T is the transposed matrix of the measurement matrix, R is the measurement noise matrix, PH is the product of the covariance matrix and the measurement matrix, HPH T is the product of the measurement matrix, the covariance matrix and the measurement transposed matrix; 1) Covariance matrix update Covariance update matrix P k =P k-T -KHP k-T , where P k-T is the covariance matrix of the solution at the previous moment, KHP k-T is the product of the gain matrix, the measurement matrix and the covariance matrix at the previous moment; 2) State matrix update State update matrix X k =X k-T +K(Z-HX k-T ), where X k-T HX is the state variable updated at the last moment. k-T is the product of the measurement matrix and the state variable of the previous moment; The corrected acceleration zero bias is converted into the acceleration zero bias in the carrier coordinate system. The formula is as follows:

7. The data fusion method of the mobile base station system based on satellite navigation according to claim 6 is characterized in that: The arm effect compensation and deviation matrix update method are as follows: Observed variable Z = [pos N POS E POS D vel N vel E vel D ], Z is the position and speed data measured in real time by the positioning module, where pos N ,pos E ,pos D are the north distance, east distance and vertical distance respectively; vel N ,vel E ,vel D are the north velocity, east velocity and vertical velocity respectively; 1) Arm effect compensation The above pos N ,pos E ,pos D ,vel N ,vel E ,vel D The data is after the arm effect compensation. The position information compensation formula is as follows: Among them, offset bX 、offset bY 、offset bZ They are the deviation values ​​of the installation position of the satellite receiving antenna 1 (main antenna) from the center of the base station host in the carrier coordinate system; pos offsetN ,pos offsetE ,pos offsetD are the position compensation values ​​of the three axes in the geographic coordinate system, θ, ψ, and φ are the pitch angle, heading angle, and roll angle respectively; The formula for speed information compensation is as follows: Among them, w x 、w y 、w z They are the three-axis angular velocity measured by the IMU module, vel offsetX ,vel offsetY ,vel offsetZ They are the velocity compensation values ​​of the three axes in the carrier coordinate system, vel offsetN ,vel offsetE ,vel offsetD They are the speed compensation values ​​of the three axes in the geographic coordinate system; The position and speed data after arm effect compensation are obtained, and the formula is as follows: 2) Deviation Matrix Update The deviation matrix E=Z-HX calculates the deviation between the measured value and the predicted value, where the measurement matrix H is the Jacobian matrix of the observed variable Z with respect to the state variable X, and HX is the product of the measurement matrix H and the state variable matrix X.

Images