Adaptive filtering Beidou navigation and inertial navigation positioning and attitude determination method and device

Abstract

The invention discloses an adaptive filtering Beidou navigation and inertial navigation positioning and attitude determination method and device, and the method comprises the steps: constructing a short-baseline carrier phase and pseudo-range double-difference observation equation through BDS satellite second pulse signals received by a first BDS receiver and a second BDS receiver, and solving the short-baseline carrier phase and pseudo-range double-difference observation equation to obtain the positioning information of a carrier where a second antenna is located; assigning the positioning information of the carrier to the inertial navigation system as an initial positioning value, carrying out positioning and attitude determination on the carrier through the inertial navigation system based on the initial positioning value, and outputting the latest positioning and attitude determination information of the carrier; carrying out Kalman filtering processing on the positioning information of the carrier and the latest positioning and attitude determination information of the carrier, and outputting the optimal attitude and positioning information of the carrier; the Kalman filtering is used for adaptively adjusting noise by using a carrier state vector through innovation analysis; the state vector is constructed by using the positioning information of the carrier and the latest positioning and attitude determination information of the carrier. The problem of error accumulation in the navigation process can be effectively solved, and the positioning precision is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of navigation technology, in particular to a self-adaptive filtering Beidou navigation and inertial navigation positioning and pose determination method and device. BACKGROUND

[0002] This section is intended to provide background or context to the embodiments of the application recited in the claims. The description herein does not constitute admission that the prior art is prior art nor does it constitute an admission of any description in this section as prior art to an application described herein and / or in another application also owned by the applicant of the present application.

[0003] In the field of navigation technology, in order to improve the accuracy of positioning and pose determination, the existing technology usually adopts the combination of inertial navigation system (INS) and satellite navigation system (such as BDS, BeiDou Navigation Satellite System). Inertial navigation system uses accelerometer to measure acceleration and gyroscope to measure angular velocity information for navigation, while satellite navigation system corrects the positioning by receiving satellite signals. However, this combined navigation system has some defects in practical application.

[0004] Firstly, the inertial navigation unit is affected by the carrier environment during the measurement of acceleration and angular velocity, such as the vibration and temperature fluctuation of the carrier, resulting in serious dynamic errors in the output information. These errors will gradually accumulate over time, eventually leading to a significant decrease in the accuracy of the navigation system. Although satellite navigation can be used for correction, the existing combined navigation system usually uses multiple antennas to ensure the positioning accuracy of satellite navigation, and the baseline between the antenna receivers is very long, which not only increases the volume of the device, but also makes it inconvenient to move, resulting in inaccurate positioning. SUMMARY

[0005] The embodiments of the present application provide a self-adaptive filtering Beidou navigation and inertial navigation positioning and pose determination method, which is based on the combined navigation positioning method of satellite navigation and inertial navigation, combined with short baseline design, effectively solves the error accumulation problem in the navigation process, and improves the positioning accuracy. The method comprises:

[0006] The short baseline carrier phase and pseudo-range double difference observation equation is constructed by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver; the short baseline carrier phase and pseudo-range double difference observation equation uses the carrier phase double difference and pseudo-range double difference extracted by the second pulse signal as the observation value, and uses the double difference integer ambiguity and the baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver as the to-be-solved parameters; the second antenna is located on the carrier;

[0007] The positioning information of the carrier is given to an inertial navigation system as a positioning initial value, and the inertial navigation system performs positioning and pose determination of the carrier based on the positioning initial value to output the latest positioning and pose determination information of the carrier; the positioning and pose determination information includes velocity, attitude angle and position;

[0008] The positioning information of the carrier is given to an inertial navigation system as a positioning initial value, and the inertial navigation system performs positioning and pose determination of the carrier based on the positioning initial value to output the latest positioning and pose determination information of the carrier; the positioning and pose determination information includes velocity, attitude angle and position;

[0009] The positioning information of the carrier is given to an inertial navigation system as a positioning initial value, and the inertial navigation system performs positioning and pose determination of the carrier based on the positioning initial value to output the latest positioning and pose determination information of the carrier; the positioning and pose determination information includes velocity, attitude angle and position;

[0010] The embodiment of the application also provides a self-adaptive filtering Beidou navigation and inertial navigation positioning and pose determination device, which is used for a combined navigation positioning mode based on satellite navigation and inertial navigation, combines short baseline design, effectively solves the error accumulation problem in the navigation process, and improves positioning precision.

[0011] The satellite navigation calculation module is used for constructing a short baseline carrier phase and pseudo-range double difference observation equation by using BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver; the short baseline carrier phase and pseudo-range double difference observation equation takes carrier phase double difference and pseudo-range double difference extracted by using the second pulse signals as observation values, and takes double difference integer ambiguity and a baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver as to-be-solved parameters; the second antenna is located on the carrier; the calculation result of the to-be-solved parameters is obtained by solving the short baseline carrier phase and pseudo-range double difference observation equation, and the positioning information of the carrier where the second antenna is located is obtained by using the calculation result and the coordinate information of the first antenna;

[0012] The inertial navigation calculation module is used for giving the positioning information of the carrier to an inertial navigation system as a positioning initial value, performing positioning and pose determination of the carrier based on the positioning initial value by the inertial navigation system, and outputting the latest positioning and pose determination information of the carrier; the positioning and pose determination information includes velocity, attitude angle and position.

[0013] The Kalman filtering module is used for performing Kalman filtering processing on the positioning information of the carrier and the latest positioning and pose determination information of the carrier, and outputting the optimal attitude and positioning information of the carrier; the Kalman filtering performs noise adaptive adjustment by using a state vector of the carrier through innovation analysis; the state vector is constructed by using the positioning information of the carrier and the latest positioning and pose determination information of the carrier.

[0014] The embodiment of the present application also provides a computer device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the adaptive filtering Beidou navigation and inertial navigation positioning and pose determination method when executing the computer program.

[0015] The embodiment of the present application also provides a computer readable storage medium, which stores a computer program, and the computer program implements the adaptive filtering Beidou navigation and inertial navigation positioning and pose determination method when executed by a processor.

[0016] The embodiment of the present application also provides a computer program product, which comprises a computer program, and the computer program implements the adaptive filtering Beidou navigation and inertial navigation positioning and pose determination method when executed by a processor.

[0017] In the embodiment of the present application, the baseline carrier phase and pseudo-range double difference observation equation is constructed, the ionospheric delay, the tropospheric delay and the satellite orbit error are similar in the short baseline range, the double difference of the short baseline carrier phase and pseudo-range double difference observation equation makes the ionospheric delay, the tropospheric delay and the satellite orbit error be almost completely eliminated, the residual error is small, the calculation precision of the short baseline carrier phase and pseudo-range double difference observation equation is high, the double difference integer ambiguity is quickly fixed, the efficiency is higher, the success rate is higher, and the reliability is better. In the short baseline range, the influence of the earth curvature can be ignored, and the calculation is simpler. At the same time, the combined navigation positioning mode of satellite navigation and inertial navigation can effectively solve the error accumulation in the inertial navigation process, and the positioning is fast. In the case that the carrier is blocked and cannot receive satellite signals for satellite positioning, the carrier can also be positioned according to the INS information alone, and the reliability is higher than that of the ordinary inertial navigation system. The Kalman filter is used to analyze the innovation, the carrier state vector is adaptively adjusted, the deviation of the gyroscope and the accelerometer is effectively corrected, and the positioning precision is further improved. BRIEF DESCRIPTION OF DRAWINGS

[0018] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without any creative effort. In the drawings:

[0019] Figure 1 It is a flowchart of the adaptive filtering Beidou navigation and inertial navigation positioning and pose determination method in the embodiment of the present application;

[0020] Figure 2 It is a processing flowchart of the inertial navigation system positioning and pose determination in the embodiment of the present application;

[0021] Figure 3 Figure 1 is a schematic diagram of an adaptive Kalman filtering process in an embodiment of the present application;

[0022] Figure 4 Figure 2 is a schematic diagram of an adaptive filtering Beidou navigation and inertial navigation positioning and attitude determination device in an embodiment of the present application;

[0023] Figure 5 Figure 3 is an example diagram of an adaptive filtering Beidou navigation and inertial navigation positioning and attitude determination device in an embodiment of the present application. DETAILED DESCRIPTION

[0024] In order to make the objectives, technical solutions and advantages of the embodiments of the present application clearer, the embodiments of the present application are further described in detail below with reference to the drawings. Herein, the illustrative embodiments of the present application and the descriptions thereof are used to explain the present application but are not intended to limit the present application.

[0025] In order to clearly describe the technical solutions of the embodiments of the present application, in the embodiments of the present application, the terms of “first”, “second”, etc. are used to distinguish the same or similar items with basically the same functions and effects, and those skilled in the art can understand that the terms of “first”, “second”, etc. do not limit the quantity and execution order.

[0026] In the technical solutions of the present application, the acquisition, storage, use, processing, etc. of data all comply with the relevant provisions of the national laws and regulations.

[0027] First, the technical terms involved in the embodiments of the present application are explained.

[0028] Inertial coordinate system (i system): represented by OX i Y i Z i . The origin of the inertial coordinate system is the center of the earth; X i and Y i are perpendicular to each other in the earth equatorial plane, respectively pointing to the corresponding stars, and OZ i is the rotation axis of the earth. The physical quantities measured by inertial devices (gyroscopes and accelerometers) are relative to the inertial system. The inertial coordinate system does not rotate with the earth rotation.

[0029] Geocentric coordinate system: the origin of the geocentric coordinate system coincides with the center of the earth. For the geocentric coordinate system OXYZ, the plane XOY is located in the equatorial plane, and XOZ is located in the meridian plane.

[0030] Geographic coordinate system: the geographic coordinate system is also called the northeast celestial coordinate system. The origin of the geographic coordinate system is located at the center of gravity of the carrier, and the O g Z g axis is perpendicular to the geodetic horizontal plane passing through the center of gravity of the carrier and points to the zenith of the carrier. Xg O g Y g The plane coincides with the horizontal ground plane passing through the center of gravity of the carrier, O g X g The axis points due east, O g Y g The axis points due north.

[0031] Navigation coordinate system (n-frame): A reference coordinate system used to determine the vehicle's navigation parameters. In inertial navigation and integrated navigation, the geographic coordinate system is usually chosen. The navigation coordinate system uses the OX coordinate system. n Y n Z n express.

[0032] Carrier coordinate system (b system): The origin O of the carrier coordinate system b Located at the center of gravity of the carrier, Y b The axis points from the head to the tail of the carrier, X b The axis points to the right side of the carrier, Z b The axis is perpendicular to the carrier X. b O b Y b flat.

[0033] Attitude angle: heading angle θ H : Carrier travel direction Y b The angle between the pitch angle and true north is positive, with true north as the reference direction and clockwise as positive. Pitch angle θ P : Carrier Z b The angle between the axis and the local horizontal plane, positive when upward. Roll angle θ R The carrier travels in the Y direction. b With axis X b The angle between the axis and the horizontal plane is positive when the carrier rotates clockwise.

[0034] Figure 1 This is a flowchart illustrating the adaptive filtering BeiDou navigation and inertial navigation positioning and attitude determination method in an embodiment of the present invention, as shown below. Figure 1 As shown, the method includes:

[0035] Step 101: Construct a short baseline carrier phase and pseudorange double-difference observation equation using the BDS satellite second pulse signal received by the first and second BDS receivers; the short baseline carrier phase and pseudorange double-difference observation equation uses the carrier phase double difference and pseudorange double difference extracted from the second pulse signal as the observed values, and the double-difference integer ambiguity and the baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver as the parameters to be determined; the second antenna is located on the carrier;

[0036] Step 102, obtaining the calculation result of the to-be-solved parameter by solving the short baseline carrier phase and pseudorange double difference observation equation, and obtaining the positioning information of the carrier where the second antenna is located by using the calculation result and the first antenna coordinate information;

[0037] Step 103, assigning the positioning information of the carrier to the inertial navigation system as the positioning initial value, and outputting the latest positioning and attitude information of the carrier by the inertial navigation system based on the positioning initial value; the positioning and attitude information includes velocity, attitude angle and position.

[0038] Step 104, performing Kalman filtering processing on the positioning information of the carrier and the latest positioning and attitude information of the carrier, and outputting the optimal attitude and positioning information of the carrier; the Kalman filtering adjusts the noise by using the state vector of the carrier adaptively through innovation analysis; the state vector is constructed by using the positioning information of the carrier and the latest positioning and attitude information of the carrier.

[0039] The adaptive filtering BDS navigation and inertial navigation positioning and attitude determination method in the embodiment will be described in detail below.

[0040] In the embodiment, the first antenna of the first BDS receiver and the second antenna of the second BDS receiver adopt a short baseline, and the short baseline refers to a baseline distance of 10m-10km.

[0041] In an embodiment, before constructing the short baseline carrier phase and pseudorange double difference observation equation by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver, the method can further include:

[0042] According to the intensity of the BDS satellite second pulse signal and the preset intensity threshold value, it is determined whether the received BDS satellite second pulse signal is valid, and the valid BDS satellite identifier and the number of valid BDS satellites are counted.

[0043] The BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver are used to construct the short baseline carrier phase and pseudorange double difference observation equation, which can include:

[0044] When the number of valid BDS satellites is greater than 4, the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver are used to construct the short baseline carrier phase and pseudorange double difference observation equation.

[0045] For example, the first and second BDS receivers determine whether the received BDS satellite signals are valid according to the strengths of the received BDS satellite signals and the set strength threshold, respectively, and count the valid BDS satellites and the number of stars; when the number of stars of the valid BDS satellites in common view of the first and second BDS receivers is greater than 4, it is determined that the positioning information obtained by using the BDS satellites is valid, and then the valid positioning information is assigned to the inertial navigation system as the positioning initial value, and the inertial navigation system performs positioning and pose determination, otherwise, the positioning is re-performed by using the BDS satellites.

[0046] In the embodiment, the first and second BDS receivers commonly view the second pulse signals of at least 4 valid BDS satellites, and positioning information of a carrier where the second antenna is located is obtained by constructing a short baseline carrier phase and pseudo-range double difference observation equation using the received second pulse signals. When it is determined that the positioning information obtained by using the BDS satellites is valid, the positioning information is recorded as valid positioning information, and the valid positioning information includes but is not limited to the velocity, longitude, latitude, height and heading angle of the carrier.

[0047] The short baseline carrier phase and pseudo-range double difference observation equation uses the carrier phase double difference and the pseudo-range double difference extracted from the second pulse signals as observation values, and uses the double difference integer ambiguity and the baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver as parameters to be solved.

[0048] The process of constructing the short baseline carrier phase and pseudo-range double difference observation equation and solving the valid positioning information of the carrier where the second antenna is located includes: setting the BDS satellites in common view of the first and second BDS receivers as n+1, the first BDS receiver observes the first observation data, and the second BDS receiver observes the second observation data, the first observation data and the second observation data respectively contain the carrier phase and the pseudo-range of the valid BDS satellites in common view; the first BDS receiver transmits the first observation data observed by it to the second BDS receiver through a data transmission link, and the second BDS receiver uses the second observation data observed by it and the received first observation data to establish a short baseline carrier phase and pseudo-range double difference observation equation of the BDS satellites in common view for relative positioning. In an embodiment, the linearized short baseline carrier phase and pseudo-range double difference observation equation is expressed as follows:

[0049] ξ = Aχ + BN + ε;

[0050] In the formula, ξ is a vector composed of carrier phase double difference observation values and pseudo-range double difference observation values, including n carrier phase double difference observation values and n pseudo-range double difference observation values, and ξ can be a vector composed of n carrier phase double difference observation values and n pseudo-range double difference observation values, and ξ ∈ R 2n , R 2n represents a 2n-dimensional real vector space;

[0051] A represents a baseline constant coefficient matrix, used for mapping the change of baseline vector to the change of double difference observation value, A ∈ R 2n ×3 , R 2n represents a 2n×3-dimensional real vector space, the row of the baseline constant coefficient matrix corresponds to the observation equation, the column of the baseline constant coefficient matrix corresponds to the component of the baseline vector, the element of the baseline constant coefficient matrix is composed of the double difference of the satellite line-of-sight direction vector, and the baseline constant coefficient matrix describes the sensitivity of the baseline vector change to the double difference observation value;

[0052] B represents an ambiguity constant coefficient matrix, used for mapping the change of double difference integer ambiguity vector to the change of double difference observation value, for the carrier phase observation row, the corresponding element is the wavelength, for the pseudorange observation row, the corresponding element is 0, and the ambiguity constant coefficient matrix describes the sensitivity of the integer ambiguity change to the double difference carrier phase observation value, B ∈ R 2n×n , R 2n represents a 2n×n-dimensional real vector space;

[0053] ε is observation noise, including n carrier phase double difference observation noise and n pseudorange double difference observation noise, ε ∈ R 2n ;

[0054] χ is a baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver, represents the coordinate difference between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver, χ ∈ R 3 , R 3 represents a 3-dimensional real vector space;

[0055] N is a double difference integer ambiguity vector, and a double difference integer ambiguity is introduced every time a pair of BDS satellites is calculated for double difference.

[0056] The linearized short baseline carrier phase and pseudorange double difference observation equation uses the time and spatial correlation of the observation errors of the first and second BDS receivers to difference the observation values to eliminate most of the observation errors.

[0057] In an embodiment, the calculation result of the to-be-solved parameter obtained by solving the short baseline carrier phase and pseudorange double difference observation equation can include:

[0058] The least square method is used to estimate the floating point solution of the baseline vector and the double difference integer ambiguity vector, and covariance information of the floating point solution of the baseline vector and the double difference integer ambiguity vector is obtained; the covariance information of the floating point solution includes: a mutual covariance matrix between the baseline vector floating point solution and the double difference integer ambiguity vector floating point solution, a covariance matrix reflecting the correlation between the baseline vector floating point solution accuracy and the baseline vector component, and a covariance matrix reflecting the correlation between the integer ambiguity vector floating point solution accuracy and the double difference integer ambiguity component;

[0059] Under the double-difference integer ambiguity constraints, the optimal double-difference integer ambiguity vector fixed solution and the baseline vector fixed solution are determined by using the covariance information.

[0060] In the embodiment, the double-difference integer ambiguity vector fixed solution and the baseline vector fixed solution are obtained by solving the double-difference observation equations of the short baseline carrier phase and the pseudorange, the second antenna coordinates are solved by using the baseline vector fixed solution and the first antenna coordinates, the second antenna is located on the carrier, and the effective positioning information of the carrier is obtained by using the coordinates of the second antenna.

[0061] In the specific implementation process of estimating the float solution of the baseline vector χ and the double-difference integer ambiguity vector N and obtaining the covariance information of the float solution of the baseline vector χ and the double-difference integer ambiguity vector N by using the least square method, the target of the least square method is to estimate the float solution of the baseline vector χ and the double-difference integer ambiguity vector N by using the following formula:

[0062]

[0063] The float solution of the baseline vector χ and the double-difference integer ambiguity vector N is respectively. The float solution of the baseline vector χ and the double-difference integer ambiguity vector N is:

[0064]

[0065] Wherein, P is the weight matrix of the observation value, A represents the baseline constant coefficient matrix, (A T PA) -1 is the covariance information of the float solution of the baseline vector χ and the double-difference integer ambiguity vector N.

[0066] Wherein, a χχ ∈R 3×3 is the covariance matrix of the baseline vector float solution , which reflects the correlation between the accuracy of the baseline vector float solution and the baseline vector components; in the embodiment, R represents the real vector space, and the upper index represents the dimension number;

[0067] a NN ∈R n×n is the covariance matrix of the double-difference integer ambiguity float solution, which reflects the correlation between the accuracy of the double-difference integer ambiguity float solution and the double-difference integer ambiguity components;

[0068] a χN ∈R 3×n is the mutual covariance matrix between the baseline vector float solution and the double-difference integer ambiguity float solution , which reflects the correlation of the estimation errors of the two.

[0069] Under the double-difference integer ambiguity constraint, the optimal double-difference integer ambiguity vector fixed solution is searched The double-difference integer ambiguity float solution is taken as the weight matrix, so that the L2 norm of the double-difference integer ambiguity weighted by the weight matrix and the double-difference integer ambiguity float solution is minimum, and the double-difference integer ambiguity vector fixed solution is

[0070] The covariance information (A T PA) -1 is used to obtain the double-difference integer ambiguity float solution The correlation between the double-difference integer ambiguity float solution error and the baseline float solution error is obtained, and the baseline vector error is estimated and compensated based on the correlation through the double-difference integer ambiguity error; the baseline vector fixed solution is obtained , that is

[0071]

[0072] The correlation (i.e. ) between the double-difference integer ambiguity float solution error and the baseline float solution error is directly calculated from the covariance information (A ), and the correction amount of the baseline vector float solution is calculated based on the correlation through the double-difference integer ambiguity error , that is, the difference between the double-difference integer ambiguity vector fixed solution and the double-difference integer ambiguity float solution is used to correct the baseline vector float solution , and the higher-precision baseline vector fixed solution is obtained

[0073] The coordinates of the second antenna are solved by using the baseline vector fixed solution and the coordinates of the first antenna, and the coordinates of the second antenna are χ b , wherein the coordinates of the first antenna are χ a .

[0074] The second antenna is located on a carrier, and the effective positioning information of the carrier is obtained by using the coordinates of the second antenna. The effective positioning information includes but is not limited to the speed, longitude, latitude, height and heading angle of the carrier and the like.

[0075] ​The BDS satellite positioning processing, ionospheric delay, tropospheric delay, satellite orbit error is similar in short baseline range, and the double difference of the short baseline carrier phase and pseudo-range double difference observation equation makes the ionospheric delay, tropospheric delay, satellite orbit error be almost completely eliminated. NN The smaller the residual error is, the more accurate the short baseline carrier phase and pseudo-range double difference observation equation is, the double difference integer ambiguity float solution is closer to integer, the double difference integer ambiguity variance is smaller, that is, a

[0076] In step 103, the positioning information of the carrier is given to the inertial navigation system as a positioning initial value, and the positioning and attitude determination of the carrier is carried out based on the positioning initial value by the inertial navigation system, and the latest positioning and attitude determination information of the carrier is output. The inertial navigation system includes an accelerometer and a gyroscope.

[0077] Figure 2 The processing flow diagram of the inertial navigation system positioning and attitude determination in the embodiment of the application is shown in Figure 2 The positioning information of the carrier is given to the inertial navigation system as a positioning initial value, and the positioning and attitude determination of the carrier is carried out based on the positioning initial value by the inertial navigation system, and the latest positioning and attitude determination information of the carrier is output. The processing flow diagram of the inertial navigation system positioning and attitude determination in the embodiment of the application is shown in

[0078] Step 201, determining the initial position, heading angle and initial speed of the carrier by using the positioning information of the carrier;

[0079] Step 202, calculating the roll angle and pitch angle of the carrier based on the output of the accelerometer, and converting the heading angle, roll angle, pitch angle and heading angle of the carrier into a quaternion;

[0080] Step 203, obtaining the first angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system output by the gyroscope;

[0081] Step 204, obtaining the second angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system by using the positioning information of the carrier;

[0082] Step 205, calculating the angular increment of the carrier relative to the navigation coordinate system by using the first angular velocity, the second angular velocity and the quaternion, and updating the quaternion by using the angular increment;

[0083] Step 206, backstepping the attitude angle by using the updated quaternion, and converting the specific force output by the accelerometer to the navigation coordinate system by using the updated quaternion;

[0084] Step 207, calculating the gravity acceleration and the Coriolis compensation term of the position where the carrier is located by using the positioning information of the carrier, constructing a velocity differential equation by using the specific force, the gravity acceleration and the Coriolis compensation term in the navigation coordinate system, and updating the carrier velocity by using the velocity differential equation;

[0085] Step 208, updating the carrier position by using the updated carrier velocity.

[0086] In the implementation, in step 204, the second angular velocity of the navigation coordinate system n relative to the inertial coordinate system i is obtained by using the positioning information of the carrier may include:

[0087] The angular velocity of the earth-fixed coordinate system e relative to the inertial coordinate system i in the navigation coordinate system n is obtained according to the latitude of the position where the carrier is located and the earth rotation angular velocity respectively represent the x, y and z direction components of the angular velocity in the inertial coordinate system i, wherein ω ie is the earth rotation angular velocity at the equator position, λ is the latitude of the position where the carrier is located, is also the earth rotation angular velocity in the navigation coordinate system n.

[0088] The earth curvature radius R m in the meridian plane at the position where the carrier is located is calculated according to the latitude λ n ;

[0089]

[0090] wherein R is the equatorial radius, and e is the eccentricity of the earth.

[0091] The angular velocity of the navigation coordinate system n relative to the earth-fixed coordinate system e in the navigation coordinate system n is calculated according to the east, north and up direction velocities of the carrier, the two earth curvature radii R m , R n at the position where the carrier is located, the latitude λ and the altitude

[0092]

[0093]

[0094] wherein, are respectively the x, y and z components of the angular velocity of the navigation coordinate system n relative to the earth-fixed coordinate system e in the navigation coordinate system n, is the east direction velocity of the carrier, is the north velocity of the carrier, h is the altitude of the carrier at the location; since the carrier is moving on the earth surface, the navigation coordinate system n is rotating relative to the earth-centered earth-fixed coordinate system e. The rotation rate of the navigation coordinate system n relative to the earth-centered earth-fixed coordinate system e is determined by the velocity of the carrier and the earth curvature at the location.

[0095] The second angular velocity of the navigation coordinate system n relative to the inertial coordinate system i in the navigation coordinate system n is calculated by The first angular velocity of the navigation coordinate system n relative to the inertial coordinate system i in the navigation coordinate system n is calculated by The angular velocity of the navigation coordinate system n relative to the inertial coordinate system i in the navigation coordinate system n is calculated by The total rotation of the navigation coordinate system n relative to the inertial coordinate system i includes the earth rotation and the rotation of the navigation coordinate system caused by the carrier movement.

[0096] In an embodiment, the step 205 calculates the angular increment of the carrier relative to the navigation coordinate system by using the first angular velocity, the second angular velocity and the quaternion, and updates the quaternion by using the angular increment, which can include:

[0097] The second angular velocity of the navigation coordinate system n relative to the inertial coordinate system i in the navigation coordinate system n is calculated by The third angular velocity of the navigation coordinate system n relative to the inertial coordinate system i in the carrier coordinate system b is calculated by

[0098]

[0099] wherein q is the quaternion before updating, q = [q0, q1, q2, q3] T , q0, q1, q2, q3 are respectively a real part and three imaginary parts of the quaternion, q* is the conjugate of the quaternion before updating, q* = [q0, -q1, -q2, -q3] T , is the quaternion multiplication, and the quaternion represents the rotation relationship from the navigation coordinate system n to the carrier coordinate system b.

[0100] The third angular velocity of the navigation coordinate system n relative to the inertial coordinate system i in the carrier coordinate system b is calculated by The first angular velocity of the carrier coordinate system b relative to the inertial coordinate system i in the carrier coordinate system b is calculated by The fourth angular velocity of the carrier coordinate system b relative to the navigation coordinate system n in the carrier coordinate system b is calculated by

[0101] The fourth angular velocity is used to calculate the angular increment and the sampling interval length. Δθ is the angular increment, and Δt is the sampling interval (also called the sampling time interval), which is determined by the sampling frequency. The higher the sampling frequency, the smaller Δt, and the higher the accuracy of the angular increment.

[0102] Construct rotation quaternions using the following formula with angular increments:

[0103]

[0104] Where Δq is a rotation quaternion;

[0105] Quaternion updates using rotation quaternions:

[0106] q′ is the updated quaternion.

[0107] In step 206, the updated quaternion is used to back-calculate the attitude angle, resulting in a new roll angle θ'. R Pitch angle θ' P and heading angle θ' H :

[0108]

[0109] θ' P =sin -1 (2(q'0q'2-q'3q'1));

[0110]

[0111] Among them, q'=[q'0, q'1, q'2, q'3] T .

[0112] In one embodiment, step 207 calculates the gravitational acceleration and Coriolis compensation term at the location of the carrier using the carrier's positioning information, constructs a velocity differential equation using the specific force, gravitational acceleration, and Coriolis compensation term in the navigation coordinate system, and updates the carrier velocity using the velocity differential equation, which may include:

[0113] Use the updated quaternions to transform the specific force output from the accelerometer to the navigation coordinate system:

[0114]

[0115] Among them, f b The specific force in the carrier coordinate system output by the accelerometer; f n The ratio in the navigation coordinate system; q′ * This is the conjugate of the updated quaternion.

[0116] The gravitational acceleration at the location of the carrier is calculated based on the equatorial radius, the carrier's latitude λ, and its altitude h.

[0117]

[0118] Where g is the gravitational acceleration at the location of the carrier, g0 is the gravitational acceleration at the equatorial sea level: g0 = 9.78, β = 0.00193185138639, and e is the Earth's eccentricity.

[0119] Using the Earth's rotational angular velocity in navigation coordinate system n Angular velocity of navigation coordinate system n relative to the geocentric coordinate system e Obtain the angular velocity ω of the carrier in the geographic coordinate system. g :

[0120] Using the angular velocity ω of the carrier in the geographic coordinate system g and navigation coordinate system n and vehicle velocity v n Find the Coriolis compensation term: ω g ×v n .

[0121] Using the ratio f in the navigation coordinate system n The velocity is updated by constructing the following velocity differential equation based on the gravitational acceleration g at the location of the carrier and the Coriolis compensation term:

[0122] This represents the derivative of the velocity of the download volume in the navigation coordinate system.

[0123] The update speed is v n ′:

[0124] In step 208, the carrier position is updated using the updated carrier velocity:

[0125] Highly Updated:

[0126] Longitude update:

[0127] Latitude Update:

[0128] It is the updated eastward velocity of the carrier. It is the updated northbound velocity of the carrier. It is the updated carrier's celestial speed. The update speed v n We obtain γ′. γ and γ′ represent the longitudes before and after the update, respectively.

[0129] The above inertial navigation and positioning processing outputs information such as the carrier's attitude angle, velocity, position, and altitude.

[0130] The positioning information of the carrier and the latest positioning and attitude information of the carrier are subjected to Kalman filtering processing in step 104, and the optimal attitude and positioning information of the carrier are output.

[0131] In the embodiment of the application, when the BDS and the INS work simultaneously for positioning, the optimal attitude and positioning information are obtained through Kalman filtering with adaptive modulation of noise covariance.

[0132] In an embodiment, the positioning information of the carrier and the latest positioning and attitude information of the carrier are subjected to Kalman filtering processing, and the optimal attitude and positioning information of the carrier are output, which can include:

[0133] The state vector and the measurement matrix are constructed by using the positioning information of the carrier and the latest positioning and attitude information of the carrier, the state equation is constructed by using the state vector, and the measurement equation is constructed by using the measurement matrix; the measurement matrix is obtained by using the difference between the carrier velocity positioning information output by the BDS and the calculated value of the inertial navigation system; the carrier velocity positioning information output by the BDS includes, for example, the effective positioning information such as the velocity, longitude, latitude, height and heading angle of the carrier; the calculated value of the inertial navigation system includes, for example, the information such as the attitude angle, velocity, position and height;

[0134] The state equation, the measurement equation and the system noise equation are subjected to Kalman filtering processing, and the optimal attitude and positioning information of the carrier are output.

[0135] In the specific implementation process, the velocity error in the northeast direction The latitude and longitude error δλ, δφ, the height error δh, the attitude angle error δθ in the northeast direction e ,δθ n ,δθ u The accelerometer zero offset d e ,d n ,d u and the gyroscope zero offset a e ,a n ,a u are taken as the state vector x of the integrated navigation, and the state vector x is represented as follows:

[0136]

[0137] The state equation of the integrated navigation is constructed as follows: where x(t) is the state vector at time t, is the derivative of the state vector at time t, and F(t) is the state transition matrix.

[0138] Each part of the state vector x can be obtained by using the positioning information of the carrier and the latest positioning and attitude information of the carrier.

[0139] For the velocity error, the state sub-equation is: East, North, and Sky can be obtained by decomposition where g n represents the acceleration of gravity;

[0140] For the latitude error, the state sub-equation is:

[0141] For the longitude error, the state sub-equation is:

[0142] For the altitude error, the state sub-equation is: respectively represent the northward velocity, eastward velocity, and skyward velocity of the navigation coordinate system n of the carrier;

[0143] For the attitude error, the state sub-equation is: a is the gyro zero offset;

[0144] For the accelerometer zero offset, the state sub-equation is: ω d respectively represent the driving white noise of the accelerometer zero offset, and represent the external disturbance or model uncertainty affecting the change of the zero offset, η d represents the attenuation coefficient of the accelerometer zero offset, which determines the speed of the zero offset attenuation.

[0145] For the gyro zero offset, the state sub-equation is: η a represents the attenuation coefficient of the gyro zero offset, which determines the speed of the zero offset attenuation, ω a represents the driving white noise of the accelerometer zero offset, and represents the external disturbance or model uncertainty affecting the change of the zero offset.

[0146] where, for the state vector and the formulas involved in the state sub-equation, for example, v n represents the velocity, δv n represents the velocity error, are the component forms of δv n , δv n is the resultant of δv , δv represents the derivative of δv n , and similarly, θ represents the attitude angle, δθ represents the attitude angle error, δθ is the resultant of δθ e , δθ n , δθ u components, represents the derivative of δθ, and the others are similarly defined.

[0147] The difference between the carrier velocity positioning information output by the BDS and the value calculated by the inertial navigation system (for example, the difference between the velocity components of the carrier velocity positioning information in the state vector and the velocity components of the value calculated by the inertial navigation system, and the difference between the attitude angle components of the carrier velocity positioning information and the attitude angle components of the value calculated by the inertial navigation system) is selected as the measurement Z, and the measurement equation of the integrated navigation is constructed as: Z(t) = Hx(t) + u(t); wherein H is a measurement matrix, u is measurement noise, u(t) is the measurement noise at time t, and the measurement noise covariance is R. The corresponding velocity and positioning states of the state vector x in the measurement matrix are 1, and the rest are 0, for example, the accelerometer bias and the gyroscope bias of the state vector are not velocity and positioning parameters, and the velocity and the attitude angle are velocity and positioning parameters.

[0148] Figure 3 A schematic diagram of the adaptive Kalman filtering process in the embodiment of the application is shown in FIG. 1, and the method comprises the following steps. Figure 3

[0149] Initializing the state vector x, the initial measurement and the system noise covariance Q, and the measurement noise covariance R to construct an initial covariance matrix P;

[0150] Time updating the state vector and the covariance matrix;

[0151] Obtaining the measurement by using the measurement equation;

[0152] Calculating the innovation and the theoretical value of the innovation covariance by using the measurement;

[0153] Updating the Kalman gain by using the theoretical value of the innovation covariance;

[0154] Updating the state vector and the covariance matrix;

[0155] Adaptive updating of the noise covariance;

[0156] Judging whether the set iteration step or convergence is reached;

[0157] If not, returning to the step of time updating the state vector and the covariance matrix;

[0158] If yes, adaptive updating of the noise covariance.

[0159] For example, the whole adaptive Kalman filtering process comprises the following steps.

[0160] Initializing the state vector, the initial measurement and the system noise covariance Q, and the measurement noise covariance R to construct an initial covariance matrix P; wherein the system noise covariance Q is obtained according to the noise self-provided by the measurement sensor according to the measurement accuracy;

[0161] For any iteration step k, the following process is performed: ​

[0162] State vector x k-1 The covariance matrix P k-1 Get x by updating the time. k|k-1 and P k|k-1 :

[0163] x k|k-1 =F k-1 x k-1 ;x k-1 Let P be the state vector for the (k-1)th iteration. k-1 Let x be the covariance matrix of the (k-1)th iteration. k|k-1 P k|k-1 Let be the estimated covariance matrices of the predicted state in the k-th iteration and the predicted state in the k-th iteration, respectively, where the same subscripts have the same meaning;

[0164] Γ is a geometric mapper for noise propagation, which transforms physical sensor noise into a mathematical representation in state space; Q k-1 Let the system noise covariance be the value at step k-1.

[0165] Obtaining the measurement Z using the measurement equation k Z k The measurement is for the k-th iteration;

[0166] Calculating new information using measurement Theoretical value of covariance S of new information k :

[0167]

[0168] S k =HP k|k-1 H T +R k ;

[0169] This is a new information sequence containing filter performance information. S k R is the theoretical value of the new information covariance. k Let be the measurement noise covariance of the k-th iteration;

[0170] Using P k|k-1 Theoretical value of covariance S of new information k Update Kalman gain K k :

[0171]

[0172] Using x k|k-1 P k|k-1 Kalman gain and innovation sequence Update the state vector and covariance matrix:

[0173]

[0174] P k =(I-K k H)P k|k-1 ;P k is the covariance matrix of the kth step, I is a unit matrix;

[0175] The adaptive update of the noise covariance is explained as follows.

[0176] Adaptive measurement noise covariance:

[0177]

[0178] wherein, are the measurement noise covariances in the kth and k+1th iterations, respectively, and k =(1-μ) / (1-μ k+1 ), μ is a forgetting factor, and μ is taken as 0.95-0.99. k+1 μ is the forgetting factor in the k+1th iteration.

[0179] Adaptive system noise covariance:

[0180]

[0181] wherein, are the system noise covariances in the kth and k+1th iterations, respectively.

[0182] wherein, if the actual innovation covariance is greater than the theoretical value S k of the innovation covariance, it indicates that the noise is underestimated, and the measurement noise covariance or the system noise covariance needs to be increased. The adaptive adjustment of the system and measurement noise makes the filter robust to noise changes, especially when the BDS signal is blocked and the measurement noise covariance is increased, or when the IMU is disturbed and the system noise covariance is increased. In the standard Kalman filter, it is assumed that the system noise and the measurement noise are known and fixed. However, in practice, these noises will change over time. The embodiments of the present application improve the precision by real-time adaptive estimation and adjustment of the system noise and the measurement noise.

[0183] The iteration is performed until the state vector converges or the set number of iterations is reached.

[0184] x k+1 and P k+1 are the optimal errors and covariance matrices obtained after Kalman filtering, and the optimal carrier attitude positioning information is obtained through the above steps of adaptive filtering.

[0185] The embodiment of the present application overcomes the shortcomings of traditional inertial navigation positioning, such as instability, susceptibility to interference and poor reliability, and has small volume and is easy to carry due to the short baseline design, the distance between the antennas is short, the error between the longitude and latitude and the altitude of the positioning and the position of the carrier is small, and the calculation is relatively simple. The embodiment of the present application adopts the BDS / INS combined navigation positioning, and the positioning is fast. In the case that the carrier is blocked and cannot receive satellite signals for BDS positioning, the carrier can also be positioned according to the INS information alone, and the reliability is higher than that of the ordinary inertial navigation system. The Kalman filter is adopted to integrate the positioning information of the BDS and the INS to obtain the optimal positioning information, the algorithm is adopted to correct the deviation of the gyroscope and the accelerometer, so that the positioning precision of the method in the embodiment of the present application is also higher than that of the general inertial navigation system.

[0186] The embodiment of the present application also provides a self-adaptive filtering BDS navigation and inertial navigation positioning and pose determination device, as described in the following embodiment. Since the principle of solving the problem of the device is similar to that of the self-adaptive filtering BDS navigation and inertial navigation positioning and pose determination method, the implementation of the device can be referred to the implementation of the self-adaptive filtering BDS navigation and inertial navigation positioning and pose determination method, and the repeated parts will not be described herein.

[0187] Figure 4 A schematic diagram of the self-adaptive filtering BDS navigation and inertial navigation positioning and pose determination device in the embodiment of the present application is shown in FIG. 4, and the device 400 comprises: Figure 4

[0188] The satellite navigation calculation module 401 is configured to construct a short baseline carrier phase and pseudo-range double difference observation equation by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver. The short baseline carrier phase and pseudo-range double difference observation equation takes the carrier phase double difference and the pseudo-range double difference extracted by the second pulse signals as observation values, and takes the double difference integer ambiguity and the baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver as to-be-solved parameters. The second antenna is located on the carrier. The calculation result of the to-be-solved parameters is obtained by solving the short baseline carrier phase and pseudo-range double difference observation equation, and the positioning information of the carrier where the second antenna is located is obtained by using the calculation result and the coordinate information of the first antenna.

[0189] The inertial navigation calculation module 402 is configured to assign the positioning information of the carrier to the inertial navigation system as a positioning initial value, perform positioning and pose determination of the carrier based on the positioning initial value by the inertial navigation system, and output the latest positioning and pose determination information of the carrier. The positioning and pose determination information includes velocity, attitude angle and position.

[0190] ​The Kalman filter module 403 is configured to perform Kalman filtering on the positioning information of the carrier and the latest positioning and attitude determination information of the carrier, and output optimal attitude and positioning information of the carrier; the Kalman filtering is performed through innovation analysis, and the state vector of the carrier is used to adaptively adjust noise; the state vector is constructed by using the positioning information of the carrier and the latest positioning and attitude determination information of the carrier.

[0191] In an embodiment, the short baseline carrier phase and pseudo-range double difference observation equation is expressed as follows:

[0192] ξ=Aχ+BN+ε;

[0193] In the formula, ξ is a vector composed of carrier phase double difference observation values and pseudo-range double difference observation values; A represents a baseline constant coefficient matrix; B represents an ambiguity constant coefficient matrix; ε is observation noise, including n carrier phase double difference observation noises and n pseudo-range double difference observation noises; χ is a baseline vector between a first antenna of a first BDS receiver and a second antenna of a second BDS receiver; N is a double difference integer ambiguity vector, and an integer ambiguity is introduced every time a double difference is calculated for a pair of BDS satellites.

[0194] In an embodiment, the satellite navigation calculation module 401 is specifically configured to:

[0195] estimate a floating point solution of the baseline vector and the double difference integer ambiguity vector by using a least square method, and obtain covariance information of the floating point solution of the baseline vector and the double difference integer ambiguity vector; the covariance information of the floating point solution includes: a mutual covariance matrix between the baseline vector floating point solution and the double difference integer ambiguity vector floating point solution, a covariance matrix reflecting the correlation between the baseline vector floating point solution accuracy and baseline vector components, and a covariance matrix reflecting the correlation between the integer ambiguity vector floating point solution accuracy and double difference integer ambiguity components;

[0196] under the constraint of the double difference integer ambiguity, the covariance information is used to determine an optimal double difference integer ambiguity vector fixed solution and a baseline vector fixed solution.

[0197] In an embodiment, the inertial navigation system includes an accelerometer and a gyroscope.

[0198] The inertial navigation calculation module 402 is specifically configured to:

[0199] determine an initial position, a heading angle and an initial speed of the carrier by using the positioning information of the carrier;

[0200] calculate a roll angle and a pitch angle of the carrier based on the output of the accelerometer, and convert the heading angle, the roll angle, the pitch angle and the heading angle of the carrier into a quaternion;

[0201] obtain a first angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system from the output of the gyroscope;

[0202] a second angular velocity of the navigation coordinate system relative to the inertial coordinate system is obtained using the positioning information of the carrier;

[0203] an angular increment of the carrier relative to the navigation coordinate system is calculated using the first angular velocity, the second angular velocity and the quaternion, and the quaternion is updated using the angular increment;

[0204] an attitude angle is back-calculated using the updated quaternion, and a specific force output by the accelerometer is converted to the navigation coordinate system using the updated quaternion;

[0205] a gravitational acceleration and a Coriolis compensation term at a position of the carrier are calculated based on the positioning information of the carrier, a velocity differential equation is constructed using the specific force in the navigation coordinate system, the gravitational acceleration and the Coriolis compensation term, and a velocity of the carrier is updated using the velocity differential equation;

[0206] a position of the carrier is updated using the updated velocity of the carrier.

[0207] In an embodiment, the inertial navigation calculation module 402 is specifically configured to:

[0208] a third angular velocity of the navigation coordinate system relative to the inertial coordinate system in the carrier coordinate system is calculated using the second angular velocity;

[0209] a fourth angular velocity of the navigation coordinate system relative to the carrier coordinate system is calculated using the third angular velocity and the first angular velocity;

[0210] an angular increment is calculated using the fourth angular velocity and a sampling interval length;

[0211] a rotation quaternion is constructed using the angular increment according to the following formula:

[0212]

[0213] wherein, Δq is the rotation quaternion, and Δθ is the angular increment;

[0214] the quaternion is updated using the rotation quaternion.

[0215] In an embodiment, the inertial navigation calculation module 402 is specifically configured to:

[0216] a specific force output by the accelerometer is converted to the navigation coordinate system using the updated quaternion;

[0217] a gravitational acceleration at a position of the carrier is calculated according to a curvature radius of the earth and a carrier latitude and altitude;

[0218] an angular velocity value of the carrier in the geographical coordinate system is obtained using an earth rotation angular velocity in the navigation coordinate system and a fifth angular velocity of the navigation coordinate system relative to the geocentric coordinate system in the navigation coordinate system:

[0219] determine a Coriolis compensation term using the angular velocity value of the carrier in the geographic coordinate system and the velocity of the carrier in the navigation coordinate system;

[0220] construct a velocity differential equation using the specific force in the navigation coordinate system, the gravity acceleration at the position of the carrier and the Coriolis compensation term to update the velocity:

[0221]

[0222] wherein, denotes the derivative of the velocity of the carrier in the navigation coordinate system, f n is the specific force in the navigation coordinate system, g denotes the gravity acceleration at the position of the carrier, ω g is the angular velocity value of the carrier in the geographic coordinate system, v n is the velocity of the carrier in the navigation coordinate system, ω g ×v n denotes determining a Coriolis compensation term.

[0223] In an embodiment, the Kalman filtering module 403 is specifically configured to:

[0224] construct a state vector and a measurement matrix using the positioning information of the carrier and the latest positioning and attitude determination information of the carrier, construct a state equation using the state vector, and construct a measurement equation using the measurement matrix; the measurement matrix is obtained using the difference between the velocity positioning information of the carrier output by the BDS and the calculated value of the inertial navigation system;

[0225] perform Kalman filtering processing using the state equation, the measurement equation and a system noise covariance equation, and output the optimal attitude and positioning information of the carrier.

[0226] In an embodiment, the state vector is represented as follows:

[0227]

[0228] wherein, is the velocity error in the northeast direction, δλ, δφ are the latitude and longitude errors, δh is the height error, δθ e ,δθ n ,δθ u is the attitude angle error in the northeast direction, d e ,d n ,d u is the accelerometer zero offset in the northeast direction, a e ,a n ,a u is the gyroscope zero offset;

[0229] The state equation is: wherein, x(t) is the state vector at time t, is the derivative of the state vector at time t, and F(t) is the state transition matrix.

[0230] Figure 5 Figure 1 is an example diagram of an adaptive filtering BDS navigation and inertial navigation positioning and pose determination device in an embodiment of the present application. The device includes at least one processing unit connected to a bus unit, a storage unit, a communication unit, an inertial navigation system, and a second BDS receiver. The storage unit is a computer readable storage medium that can be used to store software programs, computer executable programs, and modules, such as a software program, computer executable program, and module corresponding to an adaptive filtering BDS navigation and inertial navigation positioning and pose determination method in an embodiment of the present application. The processing unit implements the adaptive filtering BDS navigation and inertial navigation positioning and pose determination method by running the software program, computer executable program, and module stored in the storage unit.

[0231] An embodiment of the present application also provides a computer device including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the adaptive filtering BDS navigation and inertial navigation positioning and pose determination method when executing the computer program.

[0232] An embodiment of the present application also provides a computer readable storage medium storing a computer program, and the computer program implements the adaptive filtering BDS navigation and inertial navigation positioning and pose determination method when executed by a processor.

[0233] An embodiment of the present application also provides a computer program product including a computer program, and the computer program implements the adaptive filtering BDS navigation and inertial navigation positioning and pose determination method when executed by a processor.

[0234] An embodiment of the present application adopts a short baseline design, and ionospheric delay, tropospheric delay, and satellite orbit error are similar in the short baseline range. Double difference of the short baseline carrier phase and pseudo-range double difference observation equation makes ionospheric delay, tropospheric delay, and satellite orbit error almost completely eliminated. The smaller the residual error is, the more accurate the short baseline carrier phase and pseudo-range double difference observation equation is, the closer the double difference integer ambiguity float solution is to an integer, and the smaller the double difference integer ambiguity variance is, i.e., a NNThe smaller, the double difference integer ambiguity is fixed quickly, the efficiency is higher, the success rate is higher, and the reliability is better. In the short baseline range, the influence of the earth curvature can be ignored, and the calculation is simpler. The embodiment of the application adopts the BDS / INS integrated navigation positioning, can effectively solve the error accumulation in the INS navigation process, and the positioning is fast. In the case that the carrier is blocked and cannot receive satellite signals for BDS positioning, the carrier can also be positioned according to the INS information alone, and the reliability is higher than that of the ordinary inertial navigation system. The Kalman filter with adaptive modulation noise covariance is adopted to integrate the positioning information of the BDS and the INS to obtain optimal positioning information, and the deviation of the gyroscope and the accelerometer is effectively adaptively corrected, so that the positioning accuracy of the system is also higher than that of the general inertial navigation system.

[0235] The technical solution adopts the GPS and BDS dual-satellite system RTK positioning technology, and is also applicable to a receiver in a single mode receiving GPS or BDS signals, so that the alternative solution is a GPS / INS integrated navigation system or a BDS / INS integrated navigation system.

[0236] Those skilled in the art should understand that the embodiments of the application can be provided as a method, a system, or a computer program product. Therefore, the application can adopt a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the application can adopt a computer program product implemented on one or more computer usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing computer usable program codes.

[0237] In the embodiments provided by the application, it should be understood that the disclosed structure and method can be implemented in other ways. For example, the structure embodiments described above are only schematic. The division of the units is only a logical function division. There can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections between the units can be indirect couplings or communication connections through some interfaces, structures or units, and can be electrical, mechanical or in other forms.

[0238] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, that is, can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment.

[0239] In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware, or in the form of a software functional unit.

[0240] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate a device implemented in the flowcharts and / or block diagrams. Figure 1 one flow or multiple flows and / or blocks Figure 1 an apparatus that carries out the functions specified in one block or multiple blocks.

[0241] These computer program instructions can also be stored in a computer readable memory capable of directing the computer or other programmable data processing devices to work in a specific manner, so that the instructions stored in the computer readable memory produce a manufactured product including instruction apparatus, which implements the flowcharts and / or block diagrams. Figure 1 one flow or multiple flows and / or blocks Figure 1 an apparatus that carries out the functions specified in one block or multiple blocks.

[0242] These computer program instructions can also be loaded into a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer implemented process, so that the instructions executed on the computer or other programmable device provide a process for implementing the flowcharts and / or block diagrams. Figure 1 one flow or multiple flows and / or blocks Figure 1 an apparatus that carries out the functions specified in one block or multiple blocks.

[0243] The above described specific embodiments further illustrate the purpose, technical solutions and beneficial effects of the present application. It should be understood that the above described is only a specific embodiment of the present application, and is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.

Claims

1. A method for adaptive filtering of BDS navigation and inertial navigation positioning and pose determination, characterized in that, The method comprises: constructing a short baseline carrier phase and pseudo-range double difference observation equation by using BDS satellite second pulse signals received by a first BDS receiver and a second BDS receiver; the short baseline carrier phase and pseudo-range double difference observation equation: taking carrier phase double difference and pseudo-range double difference extracted by using second pulse signals as observation values, and taking a baseline vector between a first antenna of the first BDS receiver and a second antenna of the second BDS receiver as a to-be-solved parameter; the second antenna is located on a carrier; obtaining a calculation result of the to-be-solved parameter by solving the short baseline carrier phase and pseudo-range double difference observation equation, and obtaining positioning information of the carrier where the second antenna is located by using the calculation result and coordinate information of the first antenna; assigning the positioning information of the carrier to an inertial navigation system as positioning initial values, performing positioning and pose determination of the carrier by the inertial navigation system based on the positioning initial values, and outputting latest positioning and pose determination information of the carrier; the positioning and pose determination information comprises velocity, attitude angle, and position; performing Kalman filtering processing on the positioning information of the carrier and the latest positioning and pose determination information of the carrier, and outputting optimal attitude and positioning information of the carrier; the Kalman filtering performs noise adaptive adjustment by using a state vector of the carrier through innovation analysis; the state vector is constructed by using the positioning information of the carrier and the latest positioning and pose determination information of the carrier.

2. The method of claim 1, wherein, The short baseline carrier phase and pseudo-range double difference observation equation is expressed as follows: ξ=Aχ+BN+ε; In the formula, ξ is a vector composed of carrier phase double difference observation values and pseudo-range double difference observation values; A represents a baseline constant coefficient matrix; B represents a ambiguity constant coefficient matrix; ε is observation noise, including n carrier phase double difference observation noises and n pseudo-range double difference observation noises; χ is a baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver; N is a double difference integer ambiguity vector, and an integer ambiguity is introduced every time a pair of BDS satellites is calculated for double difference.

3. The method of claim 2, wherein, The calculation result of the to-be-solved parameter obtained by solving the short baseline carrier phase and pseudo-range double difference observation equation comprises: estimating a floating point solution of the baseline vector and the double difference integer ambiguity vector by using a least square method, and obtaining covariance information of the floating point solution of the baseline vector and the double difference integer ambiguity vector; the covariance information of the floating point solution comprises: a mutual covariance matrix between the baseline vector floating point solution and the double difference integer ambiguity vector floating point solution, a covariance matrix reflecting correlation between the baseline vector floating point solution accuracy and baseline vector components, and a covariance matrix reflecting correlation between the integer ambiguity vector floating point solution accuracy and double difference integer ambiguity components; under the constraint of the double difference integer ambiguity, the covariance information is used to determine an optimal double difference integer ambiguity vector fixed solution and a baseline vector fixed solution.

4. The method of claim 1, wherein, The inertial navigation system comprises an accelerometer and a gyroscope; the positioning information of the carrier is assigned to the inertial navigation system as positioning initial values, the positioning and pose determination of the carrier are performed by the inertial navigation system based on the positioning initial values, and the latest positioning and pose determination information of the carrier is outputted, comprising: determining an initial position, a heading angle, and an initial velocity of the carrier by using the positioning information of the carrier; calculating a roll angle and a pitch angle of the carrier based on the output of the accelerometer, and converting the heading angle, the roll angle, the pitch angle, and the heading angle of the carrier into a quaternion; Obtaining a first angular velocity of the carrier coordinate system relative to the inertial coordinate system in the carrier coordinate system under the gyro output; Obtaining a second angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system under the positioning information of the carrier; Calculating an angular increment of the carrier relative to the navigation coordinate system by using the first angular velocity, the second angular velocity and the quaternion, and updating the quaternion by using the angular increment; Obtaining the attitude angle by using the updated quaternion, and converting the specific force output by the accelerometer to the navigation coordinate system by using the updated quaternion; Calculating the gravity acceleration and the Coriolis compensation term at the position of the carrier based on the positioning information of the carrier, constructing a velocity differential equation by using the specific force in the navigation coordinate system, the gravity acceleration and the Coriolis compensation term, and updating the carrier velocity by using the velocity differential equation; Updating the carrier position by using the updated carrier velocity.

5. The method of claim 4, wherein, Calculating an angular increment of the carrier relative to the navigation coordinate system by using the first angular velocity, the second angular velocity and the quaternion, and updating the quaternion by using the angular increment, comprising: Calculating a third angular velocity of the navigation coordinate system relative to the inertial coordinate system in the carrier coordinate system under the second angular velocity; Calculating a fourth angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system under the third angular velocity and the first angular velocity; Calculating the angular increment by using the fourth angular velocity and the sampling interval length; Constructing a rotation quaternion by using the angular increment according to the following formula: Wherein, Δq is the rotation quaternion, and Δθ is the angular increment; Updating the quaternion by using the rotation quaternion.

6. The method of claim 4, wherein, Calculating the gravity acceleration and the Coriolis compensation term at the position of the carrier based on the positioning information of the carrier, constructing a velocity differential equation by using the specific force in the navigation coordinate system, the gravity acceleration and the Coriolis compensation term, and updating the carrier velocity by using the velocity differential equation, comprising: Converting the specific force output by the accelerometer to the navigation coordinate system by using the updated quaternion; Calculating the gravity acceleration at the position of the carrier according to the earth curvature radius and the carrier latitude and altitude; Obtaining the angular velocity value of the carrier in the geographical coordinate system by using the earth rotation angular velocity in the navigation coordinate system and the fifth angular velocity of the navigation coordinate system relative to the geocentric coordinate system in the navigation coordinate system: Determining the Coriolis compensation term by using the angular velocity value of the carrier in the geographical coordinate system and the carrier velocity in the navigation coordinate system; Updating the velocity by using the specific force in the navigation coordinate system, the gravity acceleration at the position of the carrier and the Coriolis compensation term to construct the following velocity differential equation: wherein denotes the derivative of the navigation frame body velocity, f n is the specific force in the navigation frame, g denotes the gravitational acceleration at the position of the body, ω g is the angular velocity value of the body in the geographical frame, v n is the body velocity in the navigation frame, ω g x v n denotes the determination of the Coriolis compensation term.

7. The method of claim 1, wherein, Performing Kalman filtering processing on the positioning information of the carrier and the latest positioning and attitude information of the carrier, and outputting the optimal attitude and positioning information of the carrier, comprising: Constructing a state vector and a measurement matrix by using the positioning information of the carrier and the latest positioning and attitude information of the carrier, constructing a state equation by using the state vector, and constructing a measurement equation by using the measurement matrix; the measurement matrix is obtained by using the difference between the carrier velocity positioning information output by the BDS and the calculated value of the inertial navigation system; Performing Kalman filtering processing by using the state equation, the measurement equation and the system noise equation, and outputting the optimal attitude and positioning information of the carrier.

8. The method of claim 7, wherein, The state vector is represented as follows: wherein, is the velocity error in the northeast celestial direction, δλ, δφ are the latitude and longitude errors, δh is the altitude error, δθ e ,δθ n ,δθ u is the attitude angle error in the northeast celestial direction, d e ,d n ,d u is the accelerometer bias in the northeast celestial direction, a e ,a n ,a u is the gyroscope bias; The state equation is: where x(t) is a state vector at time t, is a derivative of the state vector at time t, and F(t) is a state transition matrix.

9. The method of claim 1, wherein, Before constructing the short baseline carrier phase and pseudo-range double difference observation equation by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver, further comprising: According to the intensity of the BDS satellite second pulse signal and a preset intensity threshold, it is determined whether the received BDS satellite second pulse signal is valid, and valid BDS satellite identifiers and a valid number of BDS satellites are counted; The short baseline carrier phase and pseudorange double difference observation equations are constructed by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver, including: When the valid number of BDS satellites is greater than 4, the short baseline carrier phase and pseudorange double difference observation equations are constructed by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver.

10. An adaptive filtering Beidou navigation and inertial navigation positioning and pose determination device, characterized in that, Including: The satellite navigation calculation module is configured to construct the short baseline carrier phase and pseudorange double difference observation equations by using the BDS satellite second pulse signals received by the first BDS receiver and the second BDS receiver. The short baseline carrier phase and pseudorange double difference observation equations use the carrier phase double difference and the pseudorange double difference extracted from the second pulse signals as observation values, and use the double difference integer ambiguity and the baseline vector between the first antenna of the first BDS receiver and the second antenna of the second BDS receiver as to-be-solved parameters; the second antenna is located on the carrier; The calculation result of the to-be-solved parameters is obtained by solving the short baseline carrier phase and pseudorange double difference observation equations, and the positioning information of the carrier where the second antenna is located is obtained by using the calculation result and the coordinate information of the first antenna; The inertial navigation calculation module is configured to assign the positioning information of the carrier to the inertial navigation system as a positioning initial value, and to perform positioning and pose determination of the carrier based on the positioning initial value by using the inertial navigation system, and to output the latest positioning and pose determination information of the carrier; the positioning and pose determination information includes velocity, attitude angle, and position. The Kalman filtering module is configured to perform Kalman filtering processing on the positioning information of the carrier and the latest positioning and pose determination information of the carrier, and to output the optimal attitude and positioning information of the carrier; the Kalman filtering is performed by using the state vector of the carrier to adaptively adjust the noise; the state vector is constructed by using the positioning information of the carrier and the latest positioning and pose determination information of the carrier.

11. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the method of any one of claims 1 to 9.

12. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the method of any one of claims 1 to 9.

13. A computer program product, characterised in that, The computer program product includes a computer program, and the computer program is executed by the processor to implement the method of any one of claims 1 to 9.

Images