CN116295367A - Unmanned vehicle multi-source integrated navigation method based on incremental factor graph - Google Patents
Unmanned vehicle multi-source integrated navigation method based on incremental factor graph Download PDFInfo
- Publication number
- CN116295367A CN116295367A CN202211093014.3A CN202211093014A CN116295367A CN 116295367 A CN116295367 A CN 116295367A CN 202211093014 A CN202211093014 A CN 202211093014A CN 116295367 A CN116295367 A CN 116295367A
- Authority
- CN
- China
- Prior art keywords
- factor
- measurement
- navigation
- unmanned vehicle
- state
- 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/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/20—Instruments for performing navigational calculations
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention discloses an unmanned vehicle multi-source integrated navigation method based on an incremental factor graph. The method comprises the following steps: firstly, acquiring various measurement information of an unmanned vehicle through other various vehicle-mounted high-precision sensors, and determining the type of the sensor to be fused according to the environment of the unmanned vehicle; then constructing a multi-source navigation information fusion frame based on the factor graph; then abstracting the high-precision sensor into eight factor nodes, and fusing the eight factor nodes into a multi-element sensor fusion navigation frame; under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multisource sensor is completed through real-time filtering estimation and correction; and finally, outputting navigation information to navigate the unmanned vehicle. The invention can realize the comprehensive processing of the data of multiple sensors and improve the navigation real-time performance, robustness and accuracy of the navigation system.
Description
Technical Field
The invention relates to the technical field of unmanned vehicle navigation, in particular to an unmanned vehicle multi-source combined navigation method based on an incremental factor graph.
Background
The unmanned vehicle is also called an unmanned vehicle, a computer-driven vehicle or a wheeled mobile robot, and is an intelligent vehicle for realizing unmanned through a computer system. Decades of history have been in the 20 th century, and the 21 st century has shown a trend towards practical use.
The unmanned vehicle relies on cooperation of artificial intelligence, visual computing, a radar, a monitoring device and a global positioning system, so that a computer can automatically and safely operate the motor vehicle without any active operation of human beings, and the positioning navigation technology of the unmanned vehicle is the core of the whole automatic driving technology.
The existing unmanned vehicle positioning navigation technology generally adopts a multi-source combined navigation method, and the multi-source combined navigation method is a navigation method which realizes a higher-precision navigation effect by data fusion among multiple sensors and complementation of advantages and disadvantages.
In a multi-sensor data fusion system, the frequencies of the sensors are different, the error characteristics are also different, and a part of measured values are always discarded by a traditional Kalman filtering algorithm in order to keep data synchronous, so that the information is wasted.
Standard kalman filters can only solve the linearity problem, while most sensor models contain nonlinearities. In complex environments, such as urban roadways, canyons, tunnels, where satellite signals are unacceptable. If the algorithm is dependent on the guide, once the satellite is lost, the positioning accuracy becomes poor, and the normal operation of the unmanned vehicle is seriously affected.
Disclosure of Invention
The invention aims to provide an unmanned vehicle multi-source combined navigation method based on an incremental factor graph, which can effectively fuse a plurality of sensors under a satellite rejection condition and has high accuracy.
The technical solution for realizing the purpose of the invention is as follows: an unmanned vehicle multi-source integrated navigation method based on an incremental factor graph comprises the following steps:
step 3, defining a navigation system state vector as a variable node of the factor graph, defining carrier measurement information acquired by an inertial measurement unit and other vehicle-mounted sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion frame based on the factor graph;
step 4, abstracting the vehicle-mounted sensor into eight factor nodes, and fusing the eight factor nodes into a multi-element sensor fusion navigation frame;
step 5, under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multi-source sensor is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized;
and 6, outputting navigation information to navigate the unmanned vehicle.
Compared with the prior art, the invention has the remarkable advantages that: (1) Factors consisting of MEMS inertial navigation, an odometer, an altimeter, a magnetometer, a laser radar, a millimeter wave radar, an ultrasonic radar and a binocular camera are inserted into a global factor graph, a Bayesian tree-based factor graph optimization algorithm is adopted to realize data fusion and optimization estimation on variable nodes, a navigation state is taken as a variable node, a sensor model is taken as a factor node to construct a factor graph model, an intelligent optimization algorithm is used for solving the nonlinear problem to obtain optimal state estimation at each moment, and the method can be suitable for the condition of satellite rejection, so that the capability of an unmanned vehicle for coping with complex environments is improved; (2) navigation instantaneity, robustness and accuracy are improved; (3) The method has high flexibility, and can configure and combine the sensors in a plug-and-play mode, namely if the sensors fail, the sensors can be timely deleted from the factor graph framework.
Drawings
Fig. 1 is a schematic flow chart of an unmanned vehicle multi-source integrated navigation method based on an incremental factor graph.
Fig. 2 is a schematic structural diagram of a factor graph in an embodiment of the present invention.
FIG. 3 is a schematic diagram of the factor structure of each sensor in an embodiment of the present invention.
Fig. 4 is a diagram of an unmanned vehicle running track in an embodiment of the invention.
Fig. 5 is a positioning accuracy diagram of the unmanned vehicle in the embodiment of the invention.
Detailed Description
The invention will now be described in further detail with reference to the drawings and to specific examples.
Referring to fig. 1, the unmanned vehicle multi-source integrated navigation method based on the incremental factor graph provided by the invention comprises the following steps:
step 3, defining a navigation system state vector as a variable node of the factor graph, defining carrier measurement information acquired by an inertial measurement unit and other vehicle-mounted sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion frame based on the factor graph;
step 4, abstracting the vehicle-mounted sensor into eight factor nodes, and fusing the eight factor nodes into a multi-element sensor fusion navigation frame;
step 5, under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multi-source sensor is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized;
and 6, outputting navigation information to navigate the unmanned vehicle.
In the step 1, angular velocity information and specific force information of the unmanned vehicle are obtained through a gyroscope and an accelerometer in an inertial measurement unit, and various measurement information of the unmanned vehicle is obtained through other various vehicle-mounted high-precision sensors;
further, the other various vehicle-mounted high-precision sensors comprise eight sensors, namely MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR and ultrasonic radar ULTRA.
Further, in step 2, the feasibility of the sensor is judged according to the environment where the unmanned vehicle is located, and the type of the sensor to be fused is determined;
in this case, the unmanned vehicle is driven in the roadway, and the trolley is in a satellite refusing state at this time, but other sensors work well, and the rest sensors can be fused.
In step 3, a navigation system state vector is defined as a variable node of a factor graph, carrier measurement information acquired by an inertial measurement unit and other various vehicle-mounted high-precision sensors is defined as the factor node of the factor graph, and a multi-source navigation information fusion frame based on the factor graph is constructed, specifically as follows:
step 3.1, constructing a factor graph as a bipartite graph model g= (F, X, E) of the navigation estimation problem, comprising two types of nodes: is a factor node f i E F, representing a local function in factorization; the other is variable node x j E, X, represents a variable in the global multi-function; edge of the sheete ij E means if and only if the state variable node X in the factor graph j And a factor node f corresponding thereto i When the two are related, a connecting edge exists between the two;
step 3.2, setting the product of several local functions as g (x 1 ,...,x n ) The parameters of each local function are contained in a subset { x } 1 ,...,x n In }, i.e.:
wherein J is a discrete index set, X j Is { x } 1 ,...,x n Subsets of }, f j (X j ) Is a function of the parameter X j ；
Step 3.3, factorizing the formula (1) into a factor graph structure, and setting g (x) 1 ,x 2 ,x 3 ,x 4 ,x 5 ) For 5 variables of the function, g is expressed as a form of 5 factor multiplications:
g(x 1 ,x 2 ,x 3 ,x 4 ,x 5 )＝f A (x 1 )f B (x 2 )f C (x 1 ,x 2 ,x 3 )f D (x 3 ,x 4 )f E (x 3 ,x 5 ) (2)
then j= { a, B, C, D, E }, X A ＝{x 1 }，X B ＝{x 2 }，X C ＝{x 1 ,x 2 ,x 3 }，X D ＝{x 3 ,x 4 }，X E ＝{x 3 ,x 5 }. The factor graph corresponding to the formula (2) is shown in fig. 2.
Further, in step 4, the high-precision sensor is abstracted into eight factor nodes, and the eight factor nodes are fused into the multi-element sensor fusion navigation frame, and in combination with fig. 3, the following is concrete:
IMU, MAG, BAR, ODO, VIS, LIDAR, MMWR, ULTRA is abstracted into eight factor nodes, and the multi-element sensor fusion navigation framework is represented by a factor graph construction, as shown in figure 3Shown. In the figure, circles represent state variable nodes, black squares represent factor nodes, X k Representing the navigational state of the system, f representing the measured information of each sensor. f (f) prior Representing previous metrology information, f IMU Representing metrology information from an IMU, and t k Time sum t k+1 The navigational state of the moment of time is relevant. f (f) MAG 、f BAR 、f ODO 、f VIS 、f LIDAR 、f MMWR And f ULTRA Is measurement information from MAG, BAR, ODO, VIS, LIDAR, MMWR and ULTRA, respectively.
Step 4.1, setting prior information:
the a priori information can be divided into individual a priori information p (X) of different sensors and different state variables 0 ) Each a priori information may be represented by a separate a priori factor for some variables X e X 0 Is a unary factor defined as:
f prior (x)＝d(x) (3)
wherein: d (X) is X ε X 0 A cost function of a priori factors of (a);
step 4.2, formulating an inertial navigation factor according to the MEMS inertial navigation IMU measurement model:
the time update of the unmanned vehicle navigation state can be described in an abstract way by the following equation:
wherein: f (f) b And omega b Respectively representing the specific force and the angular velocity measured by the accelerometer and the gyroscope, wherein h (·) is a measurement function;
representing the measurement of MEMS inertial navigation IMU as
Two navigation states x of its connection k And x k+1 In relation, discretizing the formula (4) can obtain:
the factor node can then be expressed as:
wherein: d [. Cndot. ] represents the corresponding cost function;
by measuring functions
Reasonable prediction can be carried out to obtain an initial value x k+1 Then adding the variable nodes into the factor graph to form new variable nodes;
step 4.3, formulating an odometer factor according to an odometer ODO measurement model:
the odometer measurement model can be derived by the following formula:
wherein the method comprises the steps of
Is the state of the odometer ODO at the time t, N odo Is the measurement noise of the odometer ODO, H odo Is a measurement function of the odometer ODO, representing the relationship between the speed of the carrier and the measured value, and thus the factor of the odometer ODO can be defined as:
the odometer factor is a unitary factor, and the state variable x in the factor graph and at the corresponding time k Are connected;
step 4.4, establishing an altimeter factor according to an altimeter BAR measurement model:
the altimetric equation can be expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,
at t k Time altitude measurement, h BAR N is a measurement function BAR To measure noise, the altimeter factor corresponding to the measurement equation is:
the altimeter factor is also a unity factor, and the state variable x in the factor graph and at the corresponding time k Are connected;
and 4.5, formulating a magnetometer factor according to a magnetometer MAG measurement model:
the magnetometer measurement equation can be expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,
at t k Moment magnetometer measurement, h MAG N is a measurement function MAG To measure noise, the magnetometer factor corresponding to the measurement equation is:
the magnetometer factor is also a unity factor, the state variable x in the factor graph and at the corresponding instant k Are connected;
and 4.6, formulating a laser radar factor according to a measurement model after the laser radar LIDAR is converted into navigation information:
according to the actual measurement equation and the prior landmark point setting, the observed quantity of the laser radar LIDAR sensor can be integrated on a plurality of layers, the association between data is explicitly described by the construction of a factor graph model added to the laser radar LIDAR, the measured value is derived from the laser radar LIDAR sensor, each measurement describes the change of the position and the direction on the ground plane between two time instances, the relative posture between the pose at the moment i-1 and the pose at the moment i is realized, the altitude, the roll and the pitch are not influenced, so the laser radar LIDAR can be regarded as a binary factor, and the measurement model can be defined as:
z＝H(x i-1 ,x i )+n (13)
wherein: x is x i-1 Is the pose state at the i-1 th moment, x i The pose state at the ith moment, n is the difference value between the upper moment and the lower moment; h (·) is a relative pose measurement function of the LIDAR, representing the relationship of the pose of the carrier at two moments, so the binary factor of the LIDAR can be defined as:
f LIDAR (x i-1 ,x i )＝d(z i -H(x i-1 ,x i )) (14)
step 4.7, setting millimeter wave radar factors and ultrasonic radar factors according to a laser radar LIDAR setting method;
step 4.8, formulating binocular camera factors according to a measurement model after the binocular camera VIS is converted into navigation information:
the binocular camera metrology equation can be expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,
at t k Time depth camera measurement value, h VIS N is a measurement function VIS To measure noise;
the binocular camera factor corresponding to the measurement equation is:
the binocular camera factor is also a unity factor, in the factor graph and the state variable x at the corresponding time k Are connected.
Further, the eight factor nodes are fused into a multi-sensor fusion navigation frame, and an incremental smoothing algorithm is adopted, wherein the incremental smoothing algorithm is specifically as follows:
when the initial factor graph framework is added with new factor nodes, determining an affected part and a corresponding affected state quantity, an unaffected part and a corresponding unaffected state quantity in the Bayesian tree according to the state quantity contained in the new factor nodes;
nonlinear fusion is carried out on the affected state quantity and the state quantity contained in the new factor node, so as to obtain a new state quantity;
and connecting the state quantities which are not affected by the newly added state quantity to obtain a new factor graph frame.
Further, in step 5, under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multi-source sensor is completed through real-time filtering estimation and correction, so that the fusion of the multi-source navigation information based on the factor graph is realized, and the method specifically comprises the following steps: .
The state equation of the 18-dimensional system of the navigation system in step 5.1 is:
wherein: x (t) is a state vector, A (t) is a state coefficient matrix, G (t) is an error coefficient matrix, and W (t) is a white noise random error vector;
the system state vector X is:
the system state vector X comprises the basic navigation parameter error of the 9-dimensional inertial navigation system and 9 dimensionsThe amount of error state of the inertial meter, wherein,
for the platform error angle δV n ，δV e ，δV u Speed error in northeast direction, δL, δR, δh is latitude, longitude, altitude position error, ε bx ，ε by ，ε bz Is a random constant of a gyroscope epsilon rx ，ε ry ，ε rz For the first order Markov process of the gyroscope, </i >>
An accelerometer first order markov process;
and 5.2, selecting constraint rules of a solving process of a multi-source information fusion algorithm based on a factor graph as follows:
wherein: n is the number of factor nodes, N is a natural number, f n (. Cndot.) is an abstract function, P (X) is a joint distribution function defined on X;
step 5.3, a measurement factor node may be written as f (X k )＝L(Z k -H) in the form of a factor node obtaining the difference between the predicted and the actual measurement information, constructing a corresponding index function to obtain an estimate of the state variable, wherein: l (·) is the cost function of the estimated quantity; h is a measurement function, which is a function related to state variables, in which H can predict sensor measurements from a given state estimate; z is Z k Is the actual measurement value obtained by various sensors;
step 5.4, the MEMS inertial navigation IMU node in the factor node is different from other measurement nodes, and the measurement value Z thereof IMU Estimate X from time k k Value X used to predict time k+1 k+1 Measurement value Z IMU The expression of (2) is:
Z IMU ＝{f b ,ω b } (20)
wherein f b 、ω b And respectively measuring the obtained specific force and angular velocity by an inertial sensor, and obtaining the expression of the IMU factor node:
f IMU ＝L(X k+1 -F(X k ,Z IMU )) (21)
wherein F is a transfer function matrix of the system, X k+1 A state vector of the system at the moment k+1;
selecting a cost function L of the factor node, and minimizing the value of the cost function L to obtain:
wherein W is a properly valued positive weighting matrix,
is an estimate of the state quantity X, min is the minimum value; to satisfy the expression (22):
step 5.5, obtaining therefrom an estimate of the current state X as:
further, in step 6, navigation information is output to navigate the unmanned vehicle, specifically as follows:
and transmitting the position information fused by the multi-source sensor information to navigation software, and driving the unmanned vehicle to travel through the navigation software.
Further, under the factor graph framework, the measured value of each sensor is encoded into a factor, only the component framework graph is added when the measured value is generated, and the data fusion and the parameter estimation are completed through Bayesian reasoning on the connected factors.
Further, the MEMS inertial navigation IMU, the magnetometer MAG, the altimeter BAR, the odometer ODO, the binocular camera VIS, the laser radar LIDAR, the millimeter wave radar MMWR, and the ultrasonic radar ULTRA can be configured and combined in a plug and play manner, and if a sensor fails, the sensor can be deleted from the factor graph frame in time.
In the embodiment, the unmanned vehicle is navigated by adopting the unmanned vehicle multi-source combined navigation method based on the incremental factor graph, the navigation track of the unmanned vehicle is set to travel according to the graph of fig. 4, and the obtained positioning precision is shown in the graph of fig. 5.
As can be seen from FIG. 5, the unmanned vehicle multi-source integrated navigation method based on the incremental factor graph can conveniently process the data from the asynchronous heterogeneous sensors, expand the factor graph nodes after receiving the output data of the sensors, quickly and effectively update the system state according to the state equation and the measurement equation of the system, realize the comprehensive processing of the data of the multiple sensors, effectively improve the accuracy, the reliability and the quick configuration capability of the navigation system, and improve the navigation performance.
Claims (10)
1. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph is characterized by comprising the following steps of:
step 1, acquiring angular velocity information and specific force information of an unmanned vehicle through a gyroscope and an accelerometer in an inertial measurement unit, and acquiring various measurement information of the unmanned vehicle through other vehicle-mounted sensors;
step 2, judging the feasibility of the vehicle-mounted sensor according to the environment of the unmanned vehicle, and determining the type of the vehicle-mounted sensor to be fused;
step 3, defining a navigation system state vector as a variable node of the factor graph, defining carrier measurement information acquired by an inertial measurement unit and other vehicle-mounted sensors as factor nodes of the factor graph, and constructing a multi-source navigation information fusion frame based on the factor graph;
step 4, abstracting the vehicle-mounted sensor into eight factor nodes, and fusing the eight factor nodes into a multi-element sensor fusion navigation frame;
step 5, under the open structure framework of the factor graph, the state and measurement updating process of the system are characterized, a filtering equation is established, and the effective fusion of the information of the multi-source sensor is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized;
and 6, outputting navigation information to navigate the unmanned vehicle.
2. The method for multi-source integrated navigation of an unmanned vehicle based on incremental factor graph according to claim 1, wherein the other vehicle-mounted sensors in step 1 include eight types of sensors, such as MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR, and ultrasonic radar ULTRA.
3. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein the determining of the feasibility of the vehicle-mounted sensor according to the environment of the unmanned vehicle in step 2, determining the type of the vehicle-mounted sensor to be fused, specifically comprises the following steps:
and judging the feasibility of MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR and ultrasonic radar ULTRA sensors according to the environment of the unmanned vehicle, determining the types of the sensors to be fused, and eliminating the types of the sensors which are invalid.
4. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein the step 3 is characterized in that the navigation system state vector is defined as a variable node of the factor graph, the carrier measurement information acquired by the inertial measurement unit and other vehicle-mounted sensors is defined as a factor node of the factor graph, and a multi-source navigation information fusion frame based on the factor graph is constructed, specifically as follows:
step 3.1, constructing a factor graph as a bipartite graph model g= (F, X, E) of the navigation estimation problem, comprising two types of nodes: is a factor node f i E F, representing a local function in factorization; the other is variable node x j E, X, represents a variable in the global multi-function; edge e ij E means if and only if the state variable node X in the factor graph j And a factor node f corresponding thereto i When correlated, X j And f i A connecting edge exists between the two connecting edges;
step 3.2, setting the product of several local functions as g (x 1 ,...,x n ) The parameters of each local function are contained in a subset { x } 1 ,...,x n In }, i.e.:
wherein J is a discrete index set, X j Is { x } 1 ,...,x n Subsets of }, f j (X j ) Is a function of the parameter X j ；
Step 3.3, factorizing the formula (1) into a factor graph structure, and setting g (x) 1 ,x 2 ,x 3 ,x 4 ,x 5 ) For 5 variables of the function, g is expressed as a form of 5 factor multiplications:
g(x 1 ,x 2 ,x 3 ,x 4 ,x 5 )＝f A (x 1 )f B (x 2 )f C (x 1 ,x 2 ,x 3 )f D (x 3 ,x 4 )f E (x 3 ,x 5 ) (2)
then j= { a, B, C, D, E }, X A ＝{x 1 }，X B ＝{x 2 }，X C ＝{x 1 ,x 2 ,x 3 }，X D ＝{x 3 ,x 4 }，X E ＝{x 3 ,x 5 }。
5. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein in step 4, the vehicle-mounted sensor is abstracted into eight factor nodes, and the eight factor nodes are fused into a multi-element sensor fusion navigation frame, specifically comprising the following steps:
step 4.1, setting prior information:
the prior information is divided into individual prior information p (X) 0 ) Each a priori information is represented by a separate a priori factor for the variable X e X 0 Is a unary factor defined as:
f prior (x)＝d(x) (3)
wherein: d (X) is X ε X 0 A cost function of a priori factors of (a);
step 4.2, formulating an inertial navigation factor according to the MEMS inertial navigation IMU measurement model:
the time update of the unmanned vehicle navigation state is abstractly described by the following equation:
wherein: f (f) b And omega b Respectively representing the specific force and the angular velocity measured by the accelerometer and the gyroscope, wherein h (·) is a measurement function;
representing the measurement of MEMS inertial navigation IMU as
Two navigational states x connected to k And x k+1 In relation, discretizing the formula (4) to obtain:
the factor node at this time is expressed as:
wherein: d [. Cndot. ] represents the corresponding cost function;
by measuring functions
Predicting to obtain initial value x k+1 Then adding the variable nodes into the factor graph to form new variable nodes;
step 4.3, formulating an odometer factor according to an odometer ODO measurement model:
the odometer measurement model is derived by the following formula:
wherein the method comprises the steps of
Is the state of the odometer ODO at the time t, N odo Is the measurement noise of the odometer ODO, H odo Is a measurement function of the odometer ODO, representing the relationship between the speed of the carrier and the measured value, and thus the factor of the odometer ODO is defined as:
the odometer factor is a unitary factor, and the state variable x in the factor graph and at the corresponding time k Are connected;
step 4.4, establishing an altimeter factor according to an altimeter BAR measurement model:
the altimetric equation is expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,
at t k Time altitude measurement, h BAR N is a measurement function BAR To measure noise, the altimeter factor corresponding to the measurement equation is:
the altimeter factor is also a unity factor, and the state variable x in the factor graph and at the corresponding time k Are connected;
and 4.5, formulating a magnetometer factor according to a magnetometer MAG measurement model:
the magnetometer measurement equation is expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,
at t k Moment magnetometer measurement, h MAG N is a measurement function MAG To measure noise, the magnetometer factor corresponding to the measurement equation is:
the magnetometer factor is also a unity factor, the state variable x in the factor graph and at the corresponding instant k Are connected;
and 4.6, formulating a laser radar factor according to a measurement model after the laser radar LIDAR is converted into navigation information:
integrating the observed quantity of the laser radar LIDAR sensor at a plurality of levels according to the actual measurement equation and the prior landmark point setting, the construction of a factor graph model added to the laser radar LIDAR explicitly describes the association between the data, the measured values derived from the laser radar LIDAR sensor, each measurement describing the changes of position and orientation on the ground plane between two time instances, these changes being the relative pose between the pose at i-1 and the pose at i-time without affecting the altitude, roll and pitch, so the laser radar LIDAR is regarded as a binary factor, so the measurement model is defined as:
z＝H(x i-1 ,x i )+n (13)
wherein: x is x i-1 Is the pose state at the i-1 th moment, x i The pose state at the ith moment, n is the difference value between the upper moment and the lower moment; h (·) is a relative pose measurement function of the LIDAR of the laser radar, representing the relationship of the pose of the carrier at two moments, so the binary factor of the LIDAR of the laser radar is defined as:
f LIDAR (x i-1 ,x i )＝d(z i -H(x i-1 ,x i )) (14)
step 4.7, setting millimeter wave radar factors and ultrasonic radar factors according to a laser radar LIDAR setting method;
step 4.8, formulating binocular camera factors according to a measurement model after the binocular camera VIS is converted into navigation information:
the binocular camera measurement equation is expressed as:
wherein, the liquid crystal display device comprises a liquid crystal display device,
at t k Time depth camera measurement value, h VIS N is a measurement function VIS To measure noise;
the binocular camera factor corresponding to the measurement equation is:
the binocular camera factor is also a unity factor, in the factor graph and the state variable x at the corresponding time k Are connected.
6. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 4, wherein eight factor nodes are fused into a multi-sensor fusion navigation frame in the step 4, and an incremental smoothing algorithm is adopted, and the incremental smoothing algorithm is specifically as follows:
when the initial factor graph framework is added with new factor nodes, determining an affected part and a corresponding affected state quantity, an unaffected part and a corresponding unaffected state quantity in the Bayesian tree according to the state quantity contained in the new factor nodes;
nonlinear fusion is carried out on the affected state quantity and the state quantity contained in the new factor node, so as to obtain a new state quantity;
and connecting the state quantities which are not affected by the newly added state quantity to obtain a new factor graph frame.
7. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein in the step 5, under the open structure framework of the factor graph, the state and measurement updating process of the characterization system are established, a filtering equation is established, and the effective fusion of the multi-source sensor information is completed through real-time filtering estimation and correction, so that the multi-source navigation information fusion based on the factor graph is realized, and the method is specifically as follows:
the state equation of the 18-dimensional system of the navigation system in step 5.1 is:
wherein: x (t) is a state vector, A (t) is a state coefficient matrix, G (t) is an error coefficient matrix, and W (t) is a white noise random error vector;
the system state vector X is:
the system state vector X contains 9-dimensional inertialThe basic navigation parameter error of the guiding system and the error state quantity of the 9-dimensional inertial instrument, wherein,
for the platform error angle δV n ，δV e ，δV u Speed error in northeast direction, δL, δR, δh is latitude, longitude, altitude position error, ε bx ，ε by ，ε bz Is a random constant of a gyroscope epsilon rx ，ε ry ，ε rz For the first order Markov process of the gyroscope, </i >>
An accelerometer first order markov process;
and 5.2, selecting constraint rules of a solving process of a multi-source information fusion algorithm based on a factor graph as follows:
wherein: n is the number of factor nodes, N is a natural number, f n (. Cndot.) is an abstract function, P (X) is a joint distribution function defined on X;
step 5.3, writing f (X) to a measurement factor node k )＝L(Z k -H) form representing factor nodes obtaining differences between predicted and actual measurement information, constructing a corresponding index function to obtain an estimate of the state variable, wherein L (·) is the cost function of the estimated quantity; h is a measurement function, which is a function related to state variables, and predicts the measurement value of the sensor according to a given state estimation in the navigation frame; z is Z k Is the actual measurement value obtained by various sensors;
step 5.4, the MEMS inertial navigation IMU node in the factor node is different from other measuring nodes, and the measurement value Z IMU Estimate X from time k k Value X used to predict time k+1 k+1 Measurement value Z IMU The expression of (2) is:
Z IMU ＝{f b ,ω b } (20)
wherein f b 、ω b And respectively measuring the obtained specific force and angular velocity by an inertial sensor, and obtaining an expression of the IMU factor node:
f IMU ＝L(X k+1 -F(X k ,Z IMU )) (21)
wherein F is a transfer function matrix of the system, X k+1 A state vector of the system at the moment k+1;
selecting a cost function L of the factor node, and minimizing the value of the cost function L to obtain:
wherein W is a properly valued positive weighting matrix,
is an estimate of the state quantity X, min is the minimum value;
to satisfy the expression (22):
step 5.5, obtaining therefrom an estimate of the current state X as:
8. the method for multi-source integrated navigation of an unmanned vehicle based on an incremental factor graph according to claim 1, wherein the outputting navigation information in step 6 is used for navigating the unmanned vehicle, and specifically comprises the following steps:
and transmitting the position information fused by the multi-source sensor information to navigation software, and driving the unmanned vehicle to travel through the navigation software.
9. The unmanned vehicle multi-source integrated navigation method based on incremental factor graphs of claim 1, wherein the measured values of each sensor are encoded into a factor in the factor graph framework, and only the component framework graphs are added when the measured values are generated, and data fusion and parameter estimation are completed by using bayesian reasoning on the connected factors.
10. The unmanned vehicle multi-source integrated navigation method based on the incremental factor graph according to claim 1, wherein the MEMS inertial navigation IMU, magnetometer MAG, altimeter BAR, odometer ODO, binocular camera VIS, laser radar LIDAR, millimeter wave radar MMWR, ultrasonic radar ULTRA can be configured and combined in a plug and play manner, and if any sensor fails, it is deleted from the factor graph framework.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211093014.3A CN116295367A (en) | 2022-09-08 | 2022-09-08 | Unmanned vehicle multi-source integrated navigation method based on incremental factor graph |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211093014.3A CN116295367A (en) | 2022-09-08 | 2022-09-08 | Unmanned vehicle multi-source integrated navigation method based on incremental factor graph |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116295367A true CN116295367A (en) | 2023-06-23 |
Family
ID=86817306
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211093014.3A Pending CN116295367A (en) | 2022-09-08 | 2022-09-08 | Unmanned vehicle multi-source integrated navigation method based on incremental factor graph |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116295367A (en) |
-
2022
- 2022-09-08 CN CN202211093014.3A patent/CN116295367A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111102978B (en) | Method and device for determining vehicle motion state and electronic equipment | |
| CN111721289B (en) | Vehicle positioning method, device, equipment, storage medium and vehicle in automatic driving | |
| CN109933056B (en) | Robot navigation method based on SLAM and robot | |
| EP0570581B1 (en) | An improved accuracy sensory system for vehicle navigation | |
| CN113945206A (en) | Positioning method and device based on multi-sensor fusion | |
| US9482536B2 (en) | Pose estimation | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| Dille et al. | Outdoor downward-facing optical flow odometry with commodity sensors | |
| CN114689047B (en) | Deep learning-based integrated navigation method, device, system and storage medium | |
| CN105547300A (en) | All-source navigation system and method used for AUV (Autonomous Underwater Vehicle) | |
| Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| CN115200578A (en) | Polynomial optimization-based inertial-based navigation information fusion method and system | |
| CN113375664B (en) | Autonomous mobile device positioning method based on dynamic loading of point cloud map | |
| CN111982126B (en) | Design method of full-source BeiDou/SINS elastic state observer model | |
| CN116839591A (en) | Track tracking and positioning filtering system and fusion navigation method of rescue unmanned aerial vehicle | |
| CN116295367A (en) | Unmanned vehicle multi-source integrated navigation method based on incremental factor graph | |
| CN116625394A (en) | Robot environment sensing and path optimizing system under unknown road condition | |
| CN114046800B (en) | High-precision mileage estimation method based on double-layer filtering frame | |
| Bakambu et al. | Heading-aided odometry and range-data integration for positioning of autonomous mining vehicles | |
| CN114915913A (en) | UWB-IMU combined indoor positioning method based on sliding window factor graph | |
| CN112985385A (en) | Positioning and orientation system and positioning and orientation method applying high-precision map | |
| RU2634071C1 (en) | Method for determining navigational parameters and strapdown inertial navigation system for its implementation |
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 |