CN115371682A - Non-equidistant federal filtering multi-source integrated navigation system and method - Google Patents
Non-equidistant federal filtering multi-source integrated navigation system and method Download PDFInfo
- Publication number
- CN115371682A CN115371682A CN202211109347.0A CN202211109347A CN115371682A CN 115371682 A CN115371682 A CN 115371682A CN 202211109347 A CN202211109347 A CN 202211109347A CN 115371682 A CN115371682 A CN 115371682A
- Authority
- CN
- China
- Prior art keywords
- output
- measurement
- filter
- sins
- sub
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a non-equal interval federal filtering multi-source integrated navigation system and a method. The method comprises the following steps: firstly, carrying out self-checking on a sensor carried by a positioning vehicle, configuring a program-controlled direct-current power supply and carrying out power failure self-checking; then collecting original data of various sensors to unpack and convert the original data into navigation data; then calculating the angular speed output average value and the acceleration output average value of each shaft as zero offset; taking accelerometer data to perform initial alignment of a pitch angle and a roll angle, and acquiring a heading angle of a positioned vehicle through a sensor; converting sensor output data into usable navigation information for fusion and carrying out non-equidistant federal filtering; and finally, feeding back the filtered state error value, and displaying the state error value in real time at the terminal. The invention has the advantages of strong processing capability, strong compatibility, small volume, low power consumption, high real-time performance and strong adaptability.
Description
Technical Field
The invention relates to the technical field of multi-source integrated navigation, in particular to a non-equidistant federal filtering multi-source integrated navigation system and a method.
Background
With the development of a vehicle-mounted integrated navigation technology, the traditional federal Kalman filtering is not enough for complex vehicle-mounted integrated navigation work, on one hand, the traditional federal Kalman filtering needs the measurement information of each sensor to arrive at the same time, and the multisource integrated navigation system has different sampling rates of different sensors due to more sensors, which inevitably causes different measurement periods of each sub-filter, namely the unequal interval problem of asynchronous information of heterogeneous sensors, and influences the navigation precision of the system; on the other hand, the increase of the number and the variety of the sensors can lead to the sharp increase of resources required by data processing and positioning algorithms of the sensors, and particularly, extremely high calculation power is required for processing 2D pixel data of a vision camera and 3D point cloud data of a laser radar; in addition, the environment in the actual vehicle navigation is very complex, and extreme weather and complex road conditions may exist, so that the requirements on the sensing precision, reliability and data processing efficiency of the sensor are very high, and the traditional embedded processor ARM or DSP cannot be used for the work.
Disclosure of Invention
The invention aims to provide a non-equal interval federal filtering multi-source combined navigation system and a method, which are used for solving the problem of unequal intervals of asynchronous information of a heterogeneous sensor and improving the efficiency of data processing.
The technical solution for realizing the purpose of the invention is as follows: a non-equidistant federal filtering multi-source integrated navigation system comprises a computer, a program-controlled direct-current power supply, a data acquisition and processing unit and a sensor unit, wherein the data acquisition and processing unit comprises a multi-channel calculator acquisition card, an RS422 serial server and an RS232 serial server, and the sensor unit comprises 7 sensors of strapdown inertial navigation SINS, satellite navigation satellite system GNSS, an altimeter ALT, a magnetometer MAG, a odometer ODO, a laser radar LIDAR and binocular vision VIS;
the data acquisition and processing unit is respectively connected with the computer, and the other end of the data acquisition and processing unit is respectively connected with each sensor product in the sensor unit; the program-controlled direct-current power supply comprises a programmable direct-current power supply module and supplies power to the computer and the sensor unit.
A non-equidistant federal filtering multi-source integrated navigation method comprises the following steps:
step 1, carrying out self-checking on 7 sensors carried by a positioning vehicle to ensure that each sensor works normally;
step 2, configuring a program-controlled direct-current power supply and performing power failure self-checking to ensure the safety of a working environment;
step 3, collecting original data of various sensors, unpacking the original data, and converting the original data into navigation data;
step 4, preheating the navigation system, and taking the angular speed output data w of the 60s gyroscope after preheating is finished i And acceleration data A output by the accelerometer i Then calculating the output average value w of the angular speed of each shaft in the period b And the average value A of the acceleration output b As a zero offset;
step 5, after zero offset is removed, the accelerometer data output by the 60s accelerometer is taken to carry out the pitch angle theta 0 And roll angle gamma 0 The course angle of the positioned vehicle is obtained through other various sensors;
step 6, unpacking the output data of each sensor, processing the measured data by adopting a segmented overlapping method, further converting the data into data under a northeast coordinate system or a carrier system, and carrying out non-equidistant federal filtering;
and 7, feeding back each state error value after the Federal filtering based on unequal intervals, and displaying the state error values in real time at the terminal.
Compared with the prior art, the invention has the following remarkable advantages: (1) The Invitta board card is used as a navigation computer, so that the problems that the resources and the processing speed on a DSP or an ARM are insufficient, and a laser radar and binocular vision cannot be processed are solved; (2) The strapdown inertial navigation, the satellite navigation system, the altimeter, the magnetometer, the odometer, the laser radar and the binocular vision 7 sensors are used for carrying out non-equidistant federal filtering information fusion, so that the navigation accuracy is improved; (3) The hardware design adopts a multi-channel serial port data acquisition card, ARM is not needed for processing, the size is smaller, the power consumption is lower, the compatibility is stronger, and the data acquisition and transmission of the multi-channel serial port can be completed by means of simple software and hardware driving; (4) The method is suitable for scenes with variable vehicle-mounted navigation environments and more sensors, improves the robustness and the real-time performance of navigation, and has great practical value for autonomous navigation and positioning of the unmanned vehicle in severe environments.
Drawings
FIG. 1 is a block diagram of a system architecture of a non-equidistant federal filtering multi-source integrated navigation system of the present invention.
FIG. 2 is a system hardware structure diagram of the unequal interval federal filtering multisource integrated navigation system of the present invention.
FIG. 3 is a flow diagram of a non-equidistant federated filtering multi-source integrated navigation method according to the present invention.
Fig. 4 is a schematic representation of a federal filter structure.
Fig. 5 is a schematic diagram of segment overlap.
Detailed Description
The invention relates to a non-equidistant federal filtering multi-source integrated navigation system, which comprises a computer, a program-controlled direct-current power supply, a data acquisition and processing unit and a sensor unit, wherein the data acquisition and processing unit comprises a multi-channel calculator acquisition card, an RS422 serial server and an RS232 serial server, and the sensor unit comprises 7 sensors of strapdown inertial navigation SINS, satellite navigation satellite system (GNSS), an altimeter ALT, a magnetometer MAG, a odometer ODO, a laser radar LIDAR and binocular vision;
one end of the data acquisition and processing unit is respectively connected with the computer, and the other end of the data acquisition and processing unit is respectively connected with each sensor product in the sensor unit; the program-controlled direct-current power supply comprises a programmable direct-current power supply module and supplies power to the computer and the sensor unit.
Further, the computer adopts a Linux operating system platform.
Furthermore, the computer adopts an English WEIDA board card provided with Ubuntu 18.04.
Furthermore, a control chip, an A/D chip and a memory of the multi-channel calculator acquisition card are all arranged on the PCB and support a Linux operating system.
Furthermore, the RS422 serial server and the RS232 serial server on board of the multi-channel calculator acquisition card support the highest transmission rate of 921.6kbps, and the controller on the board provides full modem control signals to ensure compatibility with various serial peripheral devices.
Furthermore, the voltage and current thresholds of each channel of the program-controlled direct-current power supply are independently set, and the program-controlled direct-current power supply has overvoltage and overcurrent protection functions and is used by various sensors;
each path of power supply voltage is independently set and has the function of voltage and current overload protection, the power of each path of channel is 100W, the regulation range of each path of output voltage is 0-24V, the upper limit of each path of current output is more than or equal to 5A, and the peak value of ripple noise is less than 30mV.
The invention relates to a non-equidistant federal filtering multisource combined navigation method, which comprises the following steps:
step 1, carrying out self-checking on 7 sensors carried by a positioning vehicle to ensure that each sensor works normally;
step 2, configuring a program-controlled direct-current power supply and performing power failure self-checking to ensure the safety of a working environment;
step 3, collecting original data of various sensors, unpacking the original data, and converting the original data into navigation data;
step 4, preheating the navigation system, and taking the angular speed output data w of the 60s gyroscope after preheating is finished i And acceleration data A output by the accelerometer i Then calculating the output average value w of the angular speed of each shaft in the period b And the average value A of the acceleration output b As a zero offset;
step 5, after removing zero offset, taking accelerometer data output by the accelerometer for 60s to carry out pitch angle theta 0 And roll angle gamma 0 The course angle of the positioned vehicle is obtained through other various sensors;
step 6, unpacking the output data of each sensor, processing the measured data by adopting a segmented overlapping method, further converting the data into data under a northeast coordinate system or a carrier system, and carrying out non-equidistant federal filtering;
and 7, feeding back each state error value after the Federal filtering based on unequal intervals, and displaying the state error values in real time at the terminal.
Further, the step 4 specifically includes:
setting the total number of data to be N, then:
further, the step 5 is specifically as follows:
pitch angle:
transverse roll angle:
wherein:
in the formula (I), the compound is shown in the specification,
average acceleration information of the accelerometer output in x, y, z, respectively, A xi 、A yi 、A zi Acceleration and velocity information g output by the accelerometer on x, y and z respectively 0 Is the earth gravitational acceleration.
Further, the step 6 is specifically as follows:
the state equation of the system is:
in the formula (I), the compound is shown in the specification,
is an 18-dimensional state vector composed of SINS error variables, F (t) is a state transition matrix of a system, G (t) is a noise driving matrix of the system, and W (t) is a noise random error vector of the system.
Wherein:
in the formula, F N (t) is a system matrix corresponding to 9 basic navigation parameters, F M Is a diagonal matrix of random walks of accelerometers and gyroscopes, F S Is composed of a unit matrix and a 0 matrix, F G Is a specific matrix;
the system noise matrix G (t) is:
in the formula (I), the compound is shown in the specification,
rotating the matrix, wherein I is an identity matrix;
the state vector of the system is
Wherein phi e 、φ n 、φ u Attitude angle errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction are respectively; delta V e 、δV n 、δV u The speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction are respectively; delta L, delta lambda and delta h are respectively strapdownsPosition errors of latitude, longitude and altitude of the inertial navigation system are shown; epsilon bx ε by ε bz Respectively are random constant errors of the gyroscope in the x, y and z axial directions; epsilon rx 、ε ry 、ε rz Respectively the first-order Markov offset errors of the gyroscope in the x, y and z axial directions;
respectively a first-order Markov offset error of the accelerometer in three axial directions of x, y and z;
the measurement formula of each sub-filter is as follows:
the sub-filter 1: taking the difference between the position and the speed output by the SINS and the position and the speed output by the GNSSG as a system measurement vector, wherein the system measurement equation is as follows:
in the formula, L sins 、λ sins 、h sins Is weft and warp height information, V, output by SINS sinse 、V sinsn 、V sinsu Is the speed information of the geographical system download body output by the SINS in x, y, z, L gps 、λ gps 、h gps Is GNSS output weft and warp height information, V gpse 、V gpsn 、V gpsu Is the speed information in x, y, z in the geographic region, H, output by the GNSS 1 (t) is a measurement matrix, V 1 (t) is measurement noise;
the sub-filter 2: taking the difference value between the height output by the SINS and the height output by the altimeter as a system measurement vector, wherein the system measurement equation is as follows:
Z 2 (t)＝[h sins -h alt ]＝H 2 (t)X(t)+V 2 (t)
in the formula, h alt Is altitude information output by an altimeter, H 2 (t) is a measurement matrix, V 2 (t) is measurement noise;
the sub-filter 3: taking the difference value between the course angle output by the SINS and the course angle output by the magnetometer as a system measurement vector, wherein the system measurement equation is as follows:
Z 3 (t)＝[ψ sins -ψ mag ]＝H 3 (t)X(t)+V 3 (t)
in the formula, /) sins Is the heading angle information, psi, output from the SINS mag Is course angle information output by the magnetometer, H 3 (t) is a measurement matrix, V 3 (t) is measurement noise;
the sub-filter 4: taking the difference value of the SINS output speed and the speed output by the odometer as a system measurement vector, wherein the system measurement equation is as follows:
in the formula, V odoe 、V odon 、V odou Is the speed information on x, y, z in the geographic system output by the odometer;
the sub-filter 5: using the difference values of the position and the attitude output by the SINS and the position and the attitude output by the laser radar processing as system measurement vectors, wherein the system measurement equation is as follows:
in the formula, L lider 、λ lider 、h lider Is the high information of weft and warp, psi, output by the laser radar lider Course angle information processed and output by the laser radar;
the sub-filter 6: using the difference values of the position and the attitude output by the SINS and the position and the attitude output by the binocular vision processing as system measurement vectors, wherein the system measurement equation is as follows:
in the formula, L vis 、λ vis 、h vis Is the weft height information of binocular vision output vis Is twoProcessing the output course angle information by the target vision;
performing Kalman filtering on the 6 sub-filters respectively, wherein the time is updated as follows:
in the formula (I), the compound is shown in the specification,
is the state vector at time k, phi kk-1 The matrix is transferred for the system step from time k-1 to k,
is the state vector at time k-1, P kk-1 Predicting mean square error, P, for one step at time k k-1 One-step prediction of mean square error, Γ, for time k-1 k-1 For the system noise matrix, Q k-1 Is the system noise;
the measurement is updated as follows:
in the formula, K k Kalman Filter gain at time k, R k For measuring noise, I is the identity matrix, H k To observe the matrix, Z k A system observation vector is obtained;
let the measurement period of the ith sensor be T i I =1,2,3 \ 8230m, the calculation period of the main filter is T c With a fusion period of T f ，T c And T f The definition is as follows:
wherein, G (. + -.) represents to obtain the greatest common divisor, L (. + -.) represents to obtain the least common multiple, and k is defined as each calculation period T c Time scale of division, having T i ＝k i T c ，k i Are relatively prime natural numbers; when each sub-filter has simultaneous measurement updates, i.e. T f ＝T c The requirement of the Federal Kalman filtering algorithm is completely met;
for a sub-filter:
(1) When k = k p Then, when the main filter has measurement update:
(2) When k is not equal to k p Then, when the main filter is not updated by measurement, the sub-filters only perform time update:
for the main filter:
(1) When T is f ＝T c That is, when the main filter has measurement update, the main filter performs information distribution:
(2) When T is f ≠T c When the main filter is not updated, the main filter does not perform any operation;
wherein k is p When the sub-filter has new measurement update, if the current sub-filter has no new measurement information, the requirement that k is not equal to k is met p When the sub-filter is used, only time updating is carried out; if the current sub-filter has new measurement information, k = k is satisfied p The sub-filters update time and measure at the same time; for the main filter, the main filter performs information distribution only when the main filter has measurement updates.
The present invention will be described in further detail with reference to fig. 1 to 5 and specific examples.
In order to solve the unequal interval problem of heterogeneous sensor asynchronous information and the efficiency problem of data processing, the invention aims to provide a non-equal interval federal filtering multi-source combined navigation system and a method, which can realize the real-time positioning of multi-source navigation based on the non-equal interval federal filtering by utilizing 7 sensors such as Strapdown Inertial Navigation (SINS), satellite navigation system (GPS), altimeter (ALT), magnetometer (MAG), odometer (ODO), laser radar (LIDAR) and binocular Vision (VIS) on an Ubuntu18.04 platform.
1. A multi-source combined navigation system for realizing unequal interval federal filtering on Ubuntu18.04 comprises an Invitta board card capable of being installed with Ubuntu18.04, a program-controlled direct-current power supply, a data acquisition and processing unit, a test cable and a test tool.
(1) The hardware part is a multi-channel serial port data acquisition card.
The 8-port multi-serial port data acquisition card, the control chip, the A/D chip and the memory are all arranged on the PCB. The acquisition card supports a Linux operating system, and the onboard RS422 serial server and RS232 serial server support the highest transmission rate of 921.6 kbps.
(2) The multi-source navigation system based on unequal interval federal filtering has independently controlled multi-path program control direct current power supply channels, voltage and current thresholds of each channel can be independently set, the multi-source navigation system has overvoltage and overcurrent protection functions and can be used by various sensors, each path of power supply voltage is independently set and has voltage and current overload protection functions, the power of each path of channel is 100W, the adjusting range of each path of output voltage is 0-24V, the upper limit of each path of current output is not less than 5A, and the peak value of ripple noise is less than 30mV.
(3) The multi-source navigation system based on unequal interval federal filtering can acquire data of various sensors in real time, and unpack and mark the data.
2. The invention discloses a non-equidistant federal filtering multi-source integrated navigation method based on Ubuntu18.04, which can solve and process 7 sensors including laser radar and binocular vision in real time and simultaneously perform non-equidistant federal filtering based multi-source integrated navigation in real time, and the specific flow is as follows.
The method comprises the steps of installing each sensor on a vehicle body through a tool, preheating a product for 1 minute by switching on a power supply, initializing a system, establishing a positioning information storage catalog, configuring a serial port, displaying a power-off self-checking program, operating a positioning program and positioning information on a monitoring terminal in real time, and ending a positioning task.
(1) And (4) carrying out sensor self-checking on 7 sensors carried by the positioning vehicle, and eliminating hardware faults of the sensors.
(2) And a program-controlled power supply is configured and power-off self-checking is carried out, so that the safety of the working environment is guaranteed.
(3) And collecting and unpacking the original data of various sensors, and converting the original data into navigation data.
(4) Preheating the product for 1min, and taking the angular speed output data w of the 60s gyroscope after preheating i And acceleration data A output by the accelerometer i Then, the average value w is outputted for the angular velocity of each axis during this period b And the average value A of the acceleration output b As zero offset, if the total number of data is N:
(5) after zero offset calculation is finished, the accelerometer data output by the 60s accelerometer is taken to carry out pitch angle theta 0 And roll angle gamma 0 The total number of data is N, then:
pitch angle
Roll angle
Wherein
The vehicle course angle is obtained through other various sensors;
(6) unpacking and processing data output by each sensor to convert the data into data under a northeast coordinate system or a carrier system, and processing the measured data by adopting a segmented overlapping method. The time registration refers to that all sensors are time-synchronized to a unified reference time scale, and the asynchronous measurement information is registered to the same fusion time. Time synchronization of navigation systems uses UTC as a reference.
The processing procedure of the segment overlapping-based time registration method is as follows:
first, a segmentation interval T is determined S To ensure that sensor measurements occur in each segment interval, the segment interval T should be made S Is greater than or equal to the maximum sampling period T of the sensor in the system max . Then dividing the overlapping interval T Δ In principle, the overlapping interval should be made as dense as possible to increase the sampling rate, but considering that the overlapping interval is too small, not only the data update between adjacent intervals cannot be performed, but also the burden of the system calculation is increased, selecting the intermediate value of each sensor sampling period has the best effect, calculating the average value used as the overlapping interval, and if the sampling rate is greatly different, the influence of the maximum value should be eliminated.
Using the system measurement start value as the start time T 0 Each processing interval [ T ] can be determined 0 +nT Δ T 0 +nT Δ +T s ]N =0,1,2 \8230. Obtaining the measurement set of each sensor in each interval, and obtaining the measurement estimated value z in the interval by the least square method, so that the sensors are at the same time and the interval is T Δ The measured value of (a) is used as the filtering measured value of each sensor.
(7) And performing non-equidistant federal filtering.
The state equation of the system is as follows:
wherein:
the system noise vector W (t) is as follows:
the system noise matrix G (t) is:
the state vector of the system is
Wherein phi is e 、φ n 、φ u Attitude angle errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction are respectively; delta V e 、δV n 、δV u The speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction are respectively; δ L, δ λ and δ h are respectively position errors of latitude, longitude and altitude of the strapdown inertial navigation system; epsilon bx ε by ε bz Respectively are random constant errors of the gyroscope in the x, y and z axial directions; epsilon rx 、ε ry 、ε rz Respectively a first-order Markov offset error of the gyroscope in the x, y and z axial directions;
the first-order markov offset errors of the accelerometer in the three axial directions of x, y and z are respectively, and the measurement formula of each sub-filter is as follows:
the sub-filter 1 takes the position and speed calculated by SINS and the position and speed difference output by GPS as the system measurement vector, and the system measurement equation is:
and 2, a sub-filter 2, taking a height difference value output by the height meter and calculated by the SINS as a system measurement vector, wherein a system measurement equation is as follows:
Z 2 (t)＝[h ins -h alt ]＝H 2 (t)X(t)+V 2 (t)
and 3, taking the difference value between the course angle solved by the SINS and the course angle output by the magnetometer as a system measurement vector, wherein the system measurement equation is as follows:
Z 3 (t)＝[ψ sins -ψ mag ]＝H 3 (t)X(t)+V 3 (t)
and a sub-filter 4, taking the speed difference value obtained by SINS solution and output by odometer processing as a system measurement vector, wherein the system measurement equation is as follows:
and 5, a sub-filter 5, wherein the difference value between the position and the attitude solved by the SINS and the position and the attitude output by the laser radar processing is used as a system measurement vector, and a system measurement equation is as follows:
and the sub-filter 6 is used for taking the difference value between the position and the attitude obtained by the SINS solution and the position and the attitude output by the binocular vision processing as a system measurement vector, and the system measurement equation is as follows:
performing Kalman filtering on the 6 sub-filters respectively, wherein the time is updated as follows:
the measurement is updated as follows:
let the measurement period of the ith sensor be T i (i =1,2,3 \8230m), the calculation period of the main filter is T c With a fusion period of T f ，T c And T f The definition is as follows:
wherein, G (. + -.) represents to obtain the greatest common divisor, L (. + -.) represents to obtain the least common multiple, and k is defined as each calculation period T c Time scale of division, having T i ＝k i T c (k i A relatively prime natural number). When each sub-filter has a measurement update at the same time, i.e. T f ＝T c The requirement of the Federal Kalman filtering algorithm is completely met;
for the sub-filters:
(1) When k = k p Then, when the main filter has measurement update:
(2) When k is not equal to k p When the measurement of the main filter is not updated, the sub-filters only perform time updating:
for the main filter
(1) When T is f ＝T c That is, when the main filter has measurement update, the main filter performs information distribution:
(2) When T is f ≠T c When the main filter is not updated, the main filter does not perform any operation.
Wherein k is p When the sub-filter has new measurement update time, if the current sub-filter has no new measurement information, k is not equal to k p When the sub-filter is used, only time updating is carried out; if the current sub-filter has new measurement information, k = k is satisfied p The sub-filters perform both time updates and measurement updates. For the main filter, the main filter performs information distribution only when the main filter has measurement updates.
(8) And feeding back based on each state error value after non-equidistant federal filtering, and displaying in real time at the terminal.
The invention relates to a non-equidistant federal filtering multi-source combined navigation system based on Ubuntu18.04, which can realize a non-equidistant multi-source navigation real-time positioning algorithm of the federal filtering by utilizing 7 sensors such as strapdown inertial navigation, satellite navigation, altimeter, magnetometer, odometer, laser radar and binocular vision on an Ingrada board card which can be provided with the Ubuntu 18.04. The problem of limited or ARM computing power of DSP peripheral hardware is not enough, can't handle laser radar and binocular vision and reduced the volume and the consumption of system, in addition, the hardware design adopts multichannel serial ports data acquisition card, compare in each sensor of current system and need not to use ARM to handle, the volume is littleer, the consumption is lower, compatibility is stronger, rely on succinct software and hardware drive can accomplish the data acquisition and the transmission of multichannel serial ports, and whole algorithm flow has better time sequence, the reliability and the flexibility of system have been improved, it is changeable to be applicable to on-vehicle navigation environment, the more scene of sensor, the robustness and the real-time nature of navigation are strong, vehicle navigation under the adverse circumstances fixes a position and has very big practical value.
Claims (10)
1. A non-equidistant federal filtering multi-source integrated navigation system is characterized by comprising a computer, a program-controlled direct-current power supply, a data acquisition and processing unit and a sensor unit, wherein the data acquisition and processing unit comprises a multi-channel calculator acquisition card, an RS422 serial server and an RS232 serial server, and the sensor unit comprises 7 sensors of strapdown inertial navigation SINS, satellite navigation satellite system GNSS, altimeter ALT, magnetometer MAG, odometer ODO, laser radar AR and binocular vision VIS;
one end of the data acquisition and processing unit is connected with the computer, and the other end of the data acquisition and processing unit is respectively connected with each sensor product in the sensor unit; the program-controlled direct-current power supply comprises a programmable direct-current power supply module and supplies power to the computer and the sensor unit.
2. The non-equidistant federal filtered multisource integrated navigation system of claim 1, wherein the computer employs a Linux operating system platform.
3. The non-equidistant federal filtered multi-source integrated navigation system as in claim 1, wherein the computer employs an england board card equipped with ubuntu 18.04.
4. The non-equidistant federal filtering multi-source integrated navigation system as claimed in claim 1, wherein, the control chip, the a/D chip and the memory of the multi-channel calculator acquisition card are all installed on the PCB board, supporting the Linux operating system.
5. The unequal interval federal filtering multisource integrated navigation system as claimed in claim 1, wherein the RS422 serial server and the RS232 serial server on board the multi-channel calculator acquisition card support a transmission rate of up to 921.6kbps, and the on-board controller provides a full modem control signal to ensure compatibility with various serial peripheral devices.
6. The unequal interval federal filtering multisource integrated navigation system as claimed in claim 1, wherein voltage and current thresholds of each channel of the program-controlled direct-current power supply are independently set, and the system has overvoltage and overcurrent protection functions and is used by various sensors;
each path of power supply voltage is independently set and has the functions of voltage and current overload protection, the power of each path of power supply is 100W, the regulation range of each path of output voltage is 0-24V, the upper limit of each path of current output is more than or equal to 5A, and the peak value of ripple noise is less than 30mV.
7. A non-equidistant federal filtering multi-source integrated navigation method is characterized by comprising the following steps:
step 1, carrying out self-checking on 7 sensors carried by a positioning vehicle to ensure that each sensor works normally;
step 2, configuring a program-controlled direct-current power supply and performing power failure self-checking to ensure the safety of a working environment;
step 3, collecting original data of various sensors, unpacking the original data, and converting the original data into navigation data;
step 4, preheating the navigation system, and taking the angular speed output data w of the 60s gyroscope after preheating is finished i And acceleration data A output by the accelerometer i Then calculating the output average value w of the angular speed of each shaft in the period b And the average value A of the acceleration output b As a zero offset;
step 5, after zero offset is removed, the accelerometer data output by the 60s accelerometer is taken to carry out the pitch angle theta 0 And roll angle gamma 0 The course angle of the positioned vehicle is obtained through other various sensors;
step 6, unpacking the output data of each sensor, processing the measured data by adopting a segmented overlapping method, further converting the data processing into data under a northeast coordinate system or a carrier system, and carrying out non-equidistant federal filtering;
and 7, feeding back each state error value after the Federal filtering based on unequal intervals, and displaying the state error values in real time at the terminal.
8. The unequal interval federal filtering multisource combination navigation method of claim 7, wherein the step 4 is specifically as follows:
setting the total number of data as N, then:
9. the unequal interval federal filtering multisource combination navigation method of claim 7, wherein the step 5 is specifically as follows:
pitch angle:
transverse roll angle:
wherein:
in the formula (I), the compound is shown in the specification,
average acceleration information of the accelerometer output in x, y, z, respectively, A xi 、A yi 、A zi Acceleration and velocity information g output by the accelerometer on x, y and z respectively 0 Is the earth gravitational acceleration.
10. The non-equidistant federal filtering multisource combination navigation method of claim 7, wherein the step 6 is specifically as follows:
the state equation of the system is:
in the formula (I), the compound is shown in the specification,
is an 18-dimensional state vector formed by SINS error variables, F (t) is a state transition matrix of a system, G (t) is a noise driving matrix of the system, and W (t) is a noise random error vector of the system;
wherein:
in the formula, F N (t) is a system matrix corresponding to 9 basic navigation parameters, F M Is a diagonal matrix of random walks of accelerometers and gyroscopes, F S Composed of a unit matrix and a 0 matrix, F G Is a specific matrix;
the system noise matrix G (t) is:
in the formula (I), the compound is shown in the specification,
rotating the matrix, wherein I is an identity matrix;
the state vector of the system is
Wherein phi e 、φ n 、φ u In east, north and sky, respectivelyAn attitude angle error; delta V e 、δV n 、δV u The speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction are respectively; δ L, δ λ and δ h are respectively position errors of latitude, longitude and altitude of the strapdown inertial navigation system; epsilon bx ε by ε bz Respectively are random constant errors of the gyroscope in the x, y and z axial directions; epsilon rx 、ε ry 、ε rz Respectively the first-order Markov offset errors of the gyroscope in the x, y and z axial directions;
respectively the first-order Markov offset errors of the accelerometer in the x, y and z axial directions;
the measurement formula of each sub-filter is as follows:
the sub-filter 1: taking the difference between the position and the speed output by the SINS and the position and the speed output by the GNSSG as a system measurement vector, wherein the system measurement equation is as follows:
in the formula, L sins 、λ sins 、h sins Is weft and warp height information, V, output by SINS sinse 、V sinsn 、V sinsu Speed information of geographical system download body output by SINS in x, y and z, L gps 、λ gps 、h gps Is the high information of latitude and longitude, V, output by GNSS gpse 、V gpsn 、V gpsu Is the speed information in x, y, z in the geographic region, H, output by the GNSS 1 (t) is a measurement matrix, V 1 (t) is measurement noise;
the sub-filter 2: taking the difference value between the height output by the SINS and the height output by the altimeter as a system measurement vector, wherein the system measurement equation is as follows:
Z 2 (t)＝[h sins -h alt ]＝H 2 (t)X(t)+V 2 (t)
in the formula, h alt Is heightHeight information of the meter output, H 2 (t) is a measurement matrix, V 2 (t) is measurement noise;
the sub-filter 3: taking the difference value between the course angle output by the SINS and the course angle output by the magnetometer as a system measurement vector, wherein the system measurement equation is as follows:
Z 3 (t)＝[ψ sins -ψ mag ]＝H 3 (t)X(t)+V 3 (t)
in the formula, /) sins Is the course angle information, psi, of the SINS output mag Is course angle information output by the magnetometer, H 3 (t) is a measurement matrix, V 3 (t) is measurement noise;
the sub-filter 4: taking the difference value of the SINS output speed and the speed output by the odometer as a system measurement vector, wherein the system measurement equation is as follows:
in the formula, V odoe 、V odon 、V odou Is the speed information on x, y, z under the geographical system output by the odometer;
the sub-filter 5: using the difference values of the position and the attitude output by the SINS and the position and the attitude output by the laser radar as system measurement vectors, wherein the system measurement equation is as follows:
in the formula, L lider 、λ lider 、h lider Is the weft and warp height information, psi, output by the laser radar lider The course angle information is processed and output by the laser radar;
the sub-filter 6: using the difference values of the position and the attitude output by the SINS and the position and the attitude output by the binocular vision processing as system measurement vectors, wherein a system measurement equation is as follows:
in the formula, L vis 、λ vis 、h vis Is the weft and warp height information of binocular vision output, psi vis The course angle information is output by binocular vision processing;
performing Kalman filtering on the 6 sub-filters respectively, wherein the time is updated as follows:
in the formula (I), the compound is shown in the specification,
is the state vector at time k, phi kk-1 The matrix is transferred for the system step from time k-1 to k,
is the state vector at time k-1, P kk-1 Predicting mean square error, P, for one step at time k k-1 Predicting the mean square error, Γ, for one step at time k-1 k-1 For the system noise matrix, Q k-1 Is the system noise;
the measurement is updated as follows:
in the formula, K k For the Kalman filter gain at time k, R k For measuring noise, I is the identity matrix, H k To observe the matrix, Z k A system observation vector is obtained;
let the measurement period of the ith sensor be T i I =1,2,3 \ 8230m, the calculation period of the main filter is T c With a fusion period of T f ，T c And T f The definition is as follows:
wherein, G (#) is used for calculating the greatest common divisor, L (#) is used for calculating the least common multiple, and k is defined as each calculation period T c Time scale of division, having T i ＝k i T c ，k i Is a relatively prime natural number; when each sub-filter has a measurement update at the same time, i.e. T f ＝T c The requirement of the Federal Kalman filtering algorithm is completely met;
for a sub-filter:
(1) When k = k p When the main filter has measurement update:
(2) When k is not equal to k p Then, when the main filter is not updated by measurement, the sub-filters only perform time update:
for the main filter:
(1) When T is f ＝T c That is, when the main filter has measurement update, the main filter performs information distribution:
(2) When T is f ≠T c When the main filter is not updated, the main filter does not perform any operation;
wherein k is p When the sub-filter has new measurement update, if the current sub-filter has no new measurement information, the requirement that k is not equal to k is met p When the sub-filter is used, only time updating is carried out; if the current sub-filter has new measurement information,i.e. satisfies k = k p The sub-filters update time and measure at the same time; for the main filter, the main filter performs information distribution only when the main filter has measurement updates.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211109347.0A CN115371682A (en) | 2022-09-13 | 2022-09-13 | Non-equidistant federal filtering multi-source integrated navigation system and method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211109347.0A CN115371682A (en) | 2022-09-13 | 2022-09-13 | Non-equidistant federal filtering multi-source integrated navigation system and method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115371682A true CN115371682A (en) | 2022-11-22 |
Family
ID=84070676
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211109347.0A Pending CN115371682A (en) | 2022-09-13 | 2022-09-13 | Non-equidistant federal filtering multi-source integrated navigation system and method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115371682A (en) |
-
2022
- 2022-09-13 CN CN202211109347.0A patent/CN115371682A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108731667B (en) | Method and apparatus for determining speed and pose of unmanned vehicle | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| CN111811537B (en) | Error compensation method for strapdown inertial navigation and navigation system | |
| US7979231B2 (en) | Method and system for estimation of inertial sensor errors in remote inertial measurement unit | |
| CN103235328B (en) | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method | |
| US7307585B2 (en) | Integrated aeroelasticity measurement system | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN110631574B (en) | inertia/odometer/RTK multi-information fusion method | |
| US20170074678A1 (en) | Positioning and orientation data analysis system and method thereof | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| US10488432B2 (en) | Systems and methods for compensating for the absence of a sensor measurement in a heading reference system | |
| CN112525188B (en) | Combined navigation method based on federal filtering | |
| CN110989647A (en) | Multi-sensor fusion flight controller based on SoC | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN108416387B (en) | Height filtering method based on fusion data of GPS and barometer | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| RU2277696C2 (en) | Integrated satellite inertial-navigational system | |
| RU2487318C1 (en) | Platform-free inertial attitude and heading reference system based on sensitive elements of medium accuracy | |
| CN115790613B (en) | Visual information-assisted inertial/odometer combined navigation method and device | |
| CN115371682A (en) | Non-equidistant federal filtering multi-source integrated navigation system and method | |
| KR100648882B1 (en) | Apparatus and method for calculating inertia value in navigation of unmanned aerial vehicle | |
| CN116380119A (en) | Calibration method, device and system for integrated navigation | |
| CN117346794B (en) | Unmanned ship integrated navigation system and navigation method for enteromorpha tracking | |
| RU2439498C1 (en) | Complex inertial-satellite navigation system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |