CN113252048B - Navigation positioning method, navigation positioning system and computer readable storage medium - Google Patents
Navigation positioning method, navigation positioning system and computer readable storage medium Download PDFInfo
- Publication number
- CN113252048B CN113252048B CN202110515730.5A CN202110515730A CN113252048B CN 113252048 B CN113252048 B CN 113252048B CN 202110515730 A CN202110515730 A CN 202110515730A CN 113252048 B CN113252048 B CN 113252048B
- Authority
- CN
- China
- Prior art keywords
- information
- speed
- navigation
- position information
- time
- 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.)
- Active
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
The invention provides a navigation positioning method, a navigation positioning system and a computer readable storage medium, and belongs to the technical field of vehicle-mounted navigation positioning of internet of vehicles. The method comprises the steps of utilizing information sensed by an inertial sensor to complete inertial navigation information calculation; calculating speed and position information by using mileage speed acquired by a mileage meter or an OBD through the attitude information calculated by the inertial sensor; based on the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed, a Kalman filter is adopted to realize navigation information fusion processing; and performing feedback compensation on the output information of the inertial sensor according to a processing result of Kalman filtering to obtain optimal position information. According to the method and the device, the integration of relevant information is realized through the inertial sensor and the speed information provided according to the vehicle-mounted OBD interface and by combining the Kalman filtering technology, so that stable, reliable and continuous navigation positioning information can be provided for the vehicle, and the use experience of a user is improved.
Description
Technical Field
The invention relates to the technical field of vehicle-mounted navigation and positioning of Internet of vehicles, in particular to a navigation and positioning method, a navigation and positioning system and a computer readable storage medium.
Background
With the continuous development of the car networking technology, the requirements for the positioning and navigation services of the vehicle are higher and higher. Inertial navigation is an autonomous navigation technology that does not depend on external information or radiates energy to the outside, and is capable of outputting comprehensive navigation information, and is becoming more and more popular in this field. However, the error of the navigation system implemented by the inertial navigation technology increases with the increase of time, and the navigation system cannot provide accurate, reliable and continuous navigation positioning information for the vehicle with the increase of time, so that controlling the error accumulation becomes a key technical problem to be solved by the application of the inertial navigation technology.
In view of the problem of control error accumulation existing in inertial navigation, some internet of vehicles also adopt GPS navigation, which is a system for guiding users to drive according to position information provided by GPS and a planned route before navigation, but it is known that GPS calculates its position by receiving signals sent by satellites, and when the top of GPS equipment such as a terminal is blocked, the GPS equipment cannot position. Therefore, under the scene that the GPS signal is weak or invalid, accurate, reliable and continuous navigation positioning information cannot be provided for the vehicle.
Therefore, the existing navigation positioning methods can not provide accurate, reliable and continuous navigation positioning information for the vehicle.
Disclosure of Invention
In view of the above, the present invention provides a navigation positioning method, a positioning system and a computer readable storage medium, which are used to solve the problem that in the prior art, under the condition of a signal blind area or weak signal, the positioning information output by a GPS is inaccurate or even invalid, which seriously affects the monitoring of a vehicle track and cannot provide stable, reliable and continuous navigation positioning information for a vehicle.
The technical scheme adopted by the invention for solving the technical problems is as follows:
the first aspect of the present invention further provides a navigation positioning method, including the following steps:
completing inertial navigation information calculation by using information sensed by an inertial sensor to acquire position information;
calculating speed and position information by using mileage speed acquired by a mileage recorder or an OBD (on-board diagnostics) through attitude information calculated by an inertial sensor;
based on the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed, a Kalman filter is adopted to realize navigation information fusion processing, and optimal position information and sensor error estimation are obtained;
and performing feedback compensation on the output information of the inertial sensor according to the processing result of Kalman filtering to obtain the optimal position information.
In some embodiments, said performing inertial navigation information calculations using information sensed by inertial sensors to obtain position information comprises:
after the vehicle is started, acquiring horizontal acceleration data of the vehicle within a period of time through an accelerometer, and calculating a roll angle and a pitch angle by using the horizontal acceleration data as horizontal initial attitude information of inertial navigation;
adopting the position and the speed of a GPS as the initial position and the initial speed of inertial navigation;
initializing course angle information of inertial navigation by utilizing course angle information of GPS (global positioning system), and completing an attitude matrix by combining a horizontal attitude angle
The initialization calculation of (2);
the inertial navigation calculation is carried out by utilizing the information acquired by the inertial sensor at the current moment, and the inertial navigation calculation comprises an attitude matrix
And the update calculation of the speed and the position.
In some embodiments, the calculation algorithm of the velocity and position information by the attitude information calculated by the inertial sensor and the mileage speed acquired by the odometer or the OBD is as follows:
wherein the content of the first and second substances,
is the speed of the navigation coordinate system;
is attitude information;
position information at the current moment comprises longitude, latitude and height;
position information of the previous moment; h is height information; r MD And R ND Respectively calculating the main curvature radius of the meridian circle and the prime curvature radius of the prime circle by utilizing geographic information;
the OBD speeds under the navigation coordinate systems at adjacent moments are respectively; at is the OBD speed update interval, i.e., the time difference between time k-1 and time k.
In some embodiments, the method further comprises the steps of: based on the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed, the Kalman filter is adopted to realize navigation information fusion processing, and the method for obtaining the optimal position information and estimating the sensor error comprises the following steps:
when the OBD vehicle speed information is not updated in time, the information of speed, position and the like calculated by adopting the inertial sensor passes
Completing time updating;
R K a variance matrix representing the state quantities;
Q k-1 representing an observation variance matrix at time k-1;
i denotes an identity matrix.
In some embodiments, the method further comprises the step of:
at the time of updating the OBD vehicle speed information, the position information estimated from the OBD vehicle speed and the position information calculated by the inertial sensor are passed
Completing measurement updating;
wherein the measurement information
An error indicating that the OBD estimated position and the inertial sensor estimated position are at the same time; x k As state quantities, including attitude angle errorsVelocity error, position error, and sensor bias error; h k Is a measurement matrix; phi is the state transition matrix, K k Is a gain matrix; f is a system structure parameter; wherein the subscript k represents the time k, and k/k-1 represents the prediction of the time k value at the time k-1; p is position prediction information, P K/K-1 And position prediction information indicating the value of time k-1 versus time k.
In some embodiments, the method further comprises the steps of: the method for performing feedback compensation on the output information of the inertial sensor according to the processing result of Kalman filtering to obtain the optimal position information comprises the following steps:
feedback correction of the sensor is realized according to the sensor error obtained by Kalman filtering;
and updating the Kalman filter parameters for the next filtering.
A second aspect of the present invention provides a navigation positioning system, comprising: the system comprises an inertial sensor, a mileage gauge or a vehicle-mounted diagnosis system, a position information acquisition module, an information calculation module, an optimal position and error estimation module and a feedback compensation correction module;
the inertial sensor, the mileage recorder or the vehicle-mounted diagnosis system are respectively connected with a position information acquisition module, the position information acquisition module is connected with an information calculation module, the information calculation module is connected with an optimal position and error estimation module, and the optimal position and error estimation module is connected with a feedback compensation correction module;
the position information acquisition module is used for completing inertial navigation information calculation by using information sensed by the inertial sensor to acquire position information;
the information calculation module is used for calculating speed and position information by using the mileage speed acquired by a mileage meter or an OBD through the attitude information calculated by the inertial sensor;
the optimal position and error estimation module is used for realizing navigation information fusion processing by adopting a Kalman filter based on position information calculated by an inertial sensor and position information calculated by an OBD vehicle speed to obtain optimal position information and sensor error estimation;
and the feedback compensation correction module is used for performing feedback compensation correction on the output information of the inertial sensor according to the Kalman filtering processing result so as to acquire optimal position information.
In some embodiments, the position information acquisition module comprises a horizontal initial attitude information acquisition unit, an initial position and speed acquisition unit, an attitude matrix initialization calculation unit and an inertial navigation calculation unit;
the horizontal initial attitude information acquisition unit is used for acquiring horizontal acceleration data of the vehicle within a period of time through the accelerometer after the vehicle is started, and calculating a roll angle and a pitch angle by using the horizontal acceleration data as horizontal initial attitude information of inertial navigation;
the initial position and speed acquisition unit is used for adopting the position and speed of the GPS as the initial position and initial speed of inertial navigation;
the attitude matrix initialization calculation unit is used for completing initialization of the inertial navigation course angle information by utilizing the course angle information of the GPS and completing the attitude matrix by combining the horizontal attitude angle
The initialization calculation of (2);
the inertial navigation computing unit is used for performing inertial navigation computation by using information acquired by the inertial sensor at the current moment, and comprises an attitude matrix
And (4) updating and calculating speed and position.
In some embodiments, the optimal position and error estimation module includes a time update module and a metrology update module,
the time updating module is used for passing the information such as speed, position and the like calculated by adopting the inertial sensor when the OBD vehicle speed information does not complete time updating
Completing time updating;
the measurement updating module is used for updating the OBD speed information at the moment, calculating the position information according to the OBD speed and the position information calculated by the inertial sensor, and passing through
And finishing the measurement updating.
The present application also provides a computer-readable storage medium comprising a processor, a computer-readable storage medium and a computer program stored on the computer-readable storage medium, which computer program, when executed by the processor, performs the steps of the method described above.
According to the navigation positioning method, the navigation positioning system and the computer readable storage medium provided by the embodiment of the invention, fusion of relevant information such as vehicle speed information, position information and the like is realized through the inertial sensor and speed information provided according to an On Board Diagnostics (On Board Diagnostics) interface in combination with a Kalman filtering technology, so that stable, reliable and continuous navigation positioning information can be provided for a vehicle, the navigation positioning method, the navigation positioning system and the computer readable storage medium are not limited by errors caused by the strength of a Global Positioning System (GPS) signal and the service time of the inertial navigation system, and the use experience of a user is greatly improved.
Drawings
FIG. 1 is a flowchart of a navigation positioning method according to an embodiment of the present invention;
FIG. 2 is a flowchart of a navigation positioning method according to another embodiment of the present invention;
FIG. 3 is a flowchart of a method of operation of a navigation positioning method according to yet another embodiment of the present invention;
FIG. 4 is a block diagram of a navigation positioning system according to an embodiment of the present invention;
FIG. 5 is a block diagram of a navigation positioning system according to another embodiment of the present invention.
Detailed Description
In order to make the technical problems, technical solutions and advantageous effects to be solved by the present invention clearer and clearer, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The method aims at solving the problems that the error of a navigation system realized by the existing inertial navigation technology is increased along with the increase of the use time of navigation, so that accurate, reliable and continuous navigation positioning information cannot be provided for a vehicle along with the increase of the use time of navigation, and the accurate, reliable and continuous navigation positioning information cannot be provided for the vehicle in the scene that a GPS signal is weak or invalid. In vehicle-mounted applications, a odometer or an OBD (On Board Diagnostics) can provide independent vehicle speed information as speed reference information, and speed measurement errors do not increase with time, and signals are not blocked and interfered. Therefore, the inertial navigation and the mileage meter have good complementarity, and can provide accurate, reliable and continuous navigation positioning information for the vehicle under the scene that the GPS signal is weak or fails. The invention provides a navigation positioning method, a navigation positioning system and a computer readable storage medium, which realize information fusion by utilizing speed information provided by an inertial sensor and a mileage meter or a vehicle-mounted OBD (On Board Diagnostics) interface and combining with a Kalman filtering technology, thereby ensuring that stable, reliable and continuous navigation positioning information is provided for a vehicle.
Example one
Referring to fig. 1 to fig. 3, a flowchart of a navigation positioning method according to a first embodiment of the invention is shown.
The method specifically comprises the following steps:
s10, calculating inertial navigation information by using information sensed by an inertial sensor to acquire position information;
specifically, inertial Navigation initialization is completed by using accelerometer sensing information and GNSS (Global Navigation Satellite System) information on the vehicle-mounted terminal. After the initialization is completed, the inertial sensor information (acceleration and angular velocity information) is obtained in real time to complete the calculation of the attitude, the velocity and the position, please refer to fig. 2, which specifically includes the following steps:
s101, after a vehicle is started, acquiring horizontal acceleration data of the vehicle within a period of time through an accelerometer, and calculating a roll angle and a pitch angle by using the horizontal acceleration data as horizontal initial attitude information of inertial navigation;
s102, adopting the position and the speed of a GPS as the initial position and the initial speed of inertial navigation;
s103, initializing the inertial navigation course angle information by using the course angle information of the GPS, and finishing an attitude matrix by combining a horizontal attitude angle
The initialization calculation of (2);
s104, performing inertial navigation calculation by using information acquired by the inertial sensor at the current moment, wherein the information comprises an attitude matrix
Speed of rotation
And position
S20, calculating speed and position information by using the attitude information calculated by the inertial sensor and mileage speed acquired by a mileage meter or an On Board Diagnostics (OBD);
specifically, when a odometer or an OBD (On Board Diagnostics) collects speed information of a vehicle
After updating, attitude information calculated by using inertial sensor
Velocity of vehicle coordinate system
Velocity conversion to navigational coordinate system
And based on the velocity of the navigation coordinate system
The information is used to estimate vehicle position information. The vehicle speed coordinate conversion and vehicle position estimation method comprises the following steps:
wherein, the first and the second end of the pipe are connected with each other,
is the speed of the navigation coordinate system;
is attitude information;
position information at the current k moment comprises longitude, latitude and height;
position information of the previous moment; h is height information; r is MD And R ND The radius of curvature of the meridian circle and the prime curvature of the prime circle are calculated by utilizing geographic information;
the OBD speeds under the navigation coordinate systems at the adjacent moments are respectively; Δ t is the OBD speed update time interval, i.e., time k-1Time difference from time k, P being position prediction information, P K/K-1 And position prediction information indicating the value of time k-1 versus time k.
S30, based on the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed, a Kalman filter is adopted to realize navigation information fusion processing, and optimal position information and sensor error estimation are obtained;
specifically, the information fusion is carried out by using the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed based on Kalman filtering, and the method mainly comprises the following steps:
s301, when the OBD vehicle speed information is not updated in time, the speed calculated by the inertial sensor is adopted
Location, etc. information
By passing
Completing time updating;
R K a variance matrix representing the state quantities;
Q k-1 representing an observation variance matrix at time k-1;
i denotes an identity matrix.
S302, at the update time of the OBD vehicle speed information, the position information calculated according to the OBD vehicle speed and the position information calculated by the inertial sensor are passed
And finishing the measurement updating.
Wherein the measurement information
An error indicating that the OBD estimated position and the inertial sensor estimated position are at the same time; x k State quantities including attitude angle error, speed error, position error and sensor bias error; h k Is measured byMeasuring a matrix; phi is a state transition matrix; k k Is a gain matrix; gamma is a system structure parameter; where the subscript k denotes time k and k/k-1 denotes the prediction of the value at time k for time k-1.
And S40, performing feedback compensation on the output information of the inertial sensor according to a Kalman filtering processing result to obtain optimal position information.
Specifically, feedback correction of the sensor is realized according to the sensor error obtained by Kalman filtering (Kalman filtering), and the measurement accuracy of the sensor is improved. And further updating Kalman filter parameters for the next filtering. Therefore, the cyclic calculation is realized, and the real-time integrated navigation function is completed.
Specifically, feedback is performed according to a Kalman filtering result and a set threshold value.
1) When the filtering result is smaller than the threshold value, performing feedback error compensation correction on the sensor error and the navigation parameter;
2) And when the filtering result is larger than the threshold value, performing error compensation on the sensor error and the navigation parameter at least twice, wherein the feedback quantity is not larger than the set threshold value every time, namely, the compensation correction is realized for multiple times according to a certain proportion. The feedback compensation frequency at the stage is consistent with the navigation parameter resolving frequency, namely, the navigation parameter resolving is performed once while feedback correction is performed once.
According to the navigation positioning method, the navigation positioning information is stably, reliably and continuously provided for the vehicle through the inertial sensor and the speed information provided according to the On Board Diagnostics (OBD) interface, the Kalman filtering technology is combined, the limitation of errors caused by the strength of a GPS signal and the service time of an inertial navigation system is avoided, and the use experience of a user is greatly improved.
Example two
Referring to fig. 4 and 5, the navigation positioning system according to an embodiment of the present invention includes an inertial sensor 40, a mileage meter 41 (which may also be a vehicle-mounted diagnosis system), a position information obtaining module 501, an information calculating module 502, an optimal position and error estimating module 503, and a feedback compensation correcting module 504. The inertial sensor 40 and the mileage gauge 41 are respectively connected to a position information acquisition module 501, the position information acquisition module 501 is connected to an information calculation module 502, the information calculation module 502 is connected to an optimal position and error estimation module 503, and the optimal position and error estimation module 503 is connected to a feedback compensation correction module 504.
The position information acquiring module 501 is configured to complete inertial navigation information calculation by using information sensed by the inertial sensor 40 to acquire position information;
the position information acquisition module 501 includes a horizontal initial attitude information acquisition unit 5011, an initial position and velocity acquisition unit 5012, an attitude matrix initialization calculation unit 5013, and an inertial navigation calculation unit 5014.
The horizontal initial attitude information obtaining unit 5011 is configured to collect horizontal acceleration data of the vehicle within a period of time through an accelerometer after the vehicle is started, and calculate a roll angle and a pitch angle as horizontal initial attitude information of the inertial navigation by using the horizontal acceleration data.
The initial position and velocity acquisition unit 5012 is configured to use the position and velocity of the GPS as the initial position and initial velocity of the inertial navigation.
The attitude matrix initialization calculation unit 5013 is used for completing initialization of inertial navigation course angle information by utilizing course angle information of GPS and completing attitude matrix by combining horizontal attitude angle
The initialization calculation of (2).
The inertial navigation computation unit 5014 is configured to perform inertial navigation computation using information collected by the inertial sensor 40 at the current time, and includes an attitude matrix
And (4) updating and calculating speed and position.
The information calculation module 502 is configured to calculate speed and position information by using the posture information calculated by the inertial sensor 40 and the mileage speed acquired by the odometer or the OBD (On Board Diagnostics, vehicle-mounted diagnostic system) 41.
In particular, speed information as OBD is acquired
After updating, the attitude information calculated by the inertial sensor 40 is used
Velocity of vehicle coordinate system
Velocity conversion to navigational coordinate system
And based on the speed of the navigation coordinate system
The information estimates vehicle position information. The vehicle speed coordinate conversion and vehicle position estimation method comprises the following steps:
wherein, the first and the second end of the pipe are connected with each other,
position information at the current k moment comprises longitude, latitude and height;
position information of the previous k-1 moment; h is height information; r is MD And R ND Respectively calculating the main curvature radius of the meridian circle and the prime curvature radius of the prime circle by utilizing geographic information;
the OBD speeds under the navigation coordinate systems at the adjacent moments are respectively; at is the OBD speed update interval, i.e., the time difference between time k-1 and time k.
The optimal position and error estimation module 503 is configured to implement navigation information fusion processing by using a kalman filter based on position information calculated by inertial navigation and position information calculated by an OBD vehicle speed, and obtain optimal position information and sensor error estimation.
The optimal position and error estimation module 503 includes a time update module 5031 and a measurement update module 5032,
the time updating module 5031 is used for updating the time when the OBD vehicle speed information does not complete the time updating, and the information of speed, position and the like calculated by adopting inertial navigation passes
Completing time updating;
the measurement update module 5032 is configured to update the OBD vehicle speed information by calculating position information according to the OBD vehicle speed and position information calculated by inertial navigation at the time of updating the OBD vehicle speed information
And finishing the measurement updating.
Wherein the measurement information
The error of the OBD estimated position and the inertial navigation estimated position at the same time is shown; x k State quantities including attitude angle error, velocity error, position error and sensor bias error; h k Is a measurement matrix; Φ is the state transition matrix, K k Is a gain matrix; gamma is a system structure parameter; where the subscript k denotes time k and k/k-1 denotes the prediction of the value at time k for time k-1.
And the feedback compensation correction module 504 is configured to perform feedback compensation correction on the information output by the inertial sensor according to a kalman filtering processing result, so as to obtain optimal position information.
Specifically, according to a Kalman filtering processing result, feedback compensation is performed on the output information of the inertial sensor, and optimal position information is obtained. Meanwhile, feedback correction of the sensor is achieved according to the sensor error obtained through filtering, and the measurement accuracy of the sensor is improved. And updating Kalman filter parameters for next filtering. Therefore, the cyclic calculation is realized, and the real-time integrated navigation function is completed.
According to the navigation positioning system, the speed information is provided by the inertial sensor and the On-Board odometer or OBD (On-Board Diagnostics) interface, and the Kalman filtering technology is combined, so that the fusion of the related information such as the speed information and the position information is realized, the stable, reliable and continuous navigation positioning information can be provided for the vehicle, the limitation of errors caused by the strength of GPS signals and the service time of the inertial navigation system is avoided, and the use experience of a user is greatly improved.
EXAMPLE III
According to an embodiment of the present invention, a computer-readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the steps in the navigation and positioning method are implemented, and specific steps are described in the first embodiment, and are not described herein again.
The memory in the present embodiment may be used to store software programs as well as various data. The memory may mainly include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function, and the like; the storage data area may store data created according to the use of the mobile phone, and the like. Further, the memory may include high speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other volatile solid state storage device.
According to an example of this embodiment, all or part of the processes in the methods of the embodiments described above may be implemented by a computer program to instruct related hardware, where the program may be stored in a computer-readable storage medium, and in this embodiment of the present invention, the program may be stored in the storage medium of a computer system and executed by at least one processor in the computer system, so as to implement the processes including the embodiments of the methods described above. The storage medium includes, but is not limited to, a magnetic disk, a flash disk, an optical disk, a Read-Only Memory (ROM), and the like.
The preferred embodiments of the present invention have been described above with reference to the accompanying drawings, and are not to be construed as limiting the scope of the invention. Those skilled in the art can implement the invention in various modifications, such as features from one embodiment can be used in another embodiment to yield yet a further embodiment, without departing from the scope and spirit of the invention. Any modification, equivalent replacement and improvement made within the technical idea of using the present invention should be within the scope of the right of the present invention.
Claims (7)
1. A navigation positioning method is characterized by comprising the following steps:
completing inertial navigation information calculation by using information sensed by an inertial sensor to acquire position information;
calculating speed and position information by using mileage speed acquired by a mileage meter or an OBD through the attitude information calculated by the inertial sensor;
based on the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed, a Kalman filter is adopted to realize navigation information fusion processing, and optimal position information and sensor error estimation are obtained;
according to the result of processing by Kalman filtering, performing feedback compensation on the output information of the inertial sensor to obtain optimal position information;
the method for estimating the optimal position information and the sensor error based on the position information calculated by the inertial sensor and the position information calculated by the OBD vehicle speed adopts a Kalman filter to realize navigation information fusion processing and comprises the following steps:
when the OBD vehicle speed information is not updated in time, the speed and position information calculated by the inertial sensor passes
Completing time updating;
at the time of updating the OBD vehicle speed information, the position information estimated from the OBD vehicle speed and the position information calculated by the inertial sensor are passed
Completing measurement updating;
wherein the measurement information
An error indicating that the OBD estimated position and the inertial sensor estimated position are at the same time; x k State quantities including attitude angle error, velocity error, position error and sensor bias error; h k Is a measurement matrix; phi is the state transition matrix, K k Is a gain matrix; f is a system structure parameter; wherein the subscript k represents the time k, and k/k-1 represents the prediction of the time k value at the time k-1;
p is position prediction information, P K/K-1 Position prediction information representing the value at time k-1 versus time k,
R K a variance matrix representing the state quantities;
Q k-1 representing an observation variance matrix at time k-1;
i denotes an identity matrix.
2. The navigation positioning method according to claim 1, wherein the performing inertial navigation information calculation using information sensed by the inertial sensor and obtaining position information comprises:
after the vehicle is started, acquiring horizontal acceleration data of the vehicle within a period of time through an accelerometer, and calculating a roll angle and a pitch angle by using the horizontal acceleration data as horizontal initial attitude information of inertial navigation;
adopting the position and the speed of the GPS as the initial position and the initial speed of inertial navigation;
the course angle information of the GPS is utilized to complete the initialization of the course angle information of the inertial navigation, and the attitude matrix is completed by combining the horizontal attitude angle
The initialization calculation of (2);
the inertial navigation calculation is carried out by utilizing the information acquired by the inertial sensor at the current moment, and the inertial navigation calculation comprises an attitude matrix
And (4) updating and calculating the speed and the position.
3. The navigation positioning method according to claim 1, wherein the attitude information calculated by the inertial sensor is calculated by using a mileage speed completion speed and position information collected by a odometer or an OBD as follows:
wherein, the first and the second end of the pipe are connected with each other,
is the speed of the navigation coordinate system;
is attitude information;
position information at the current k moment comprises longitude, latitude and height;
position information of the previous moment; h is height information; r MD And R ND Respectively calculating the main curvature radius of the meridian circle and the prime curvature radius of the prime circle by utilizing geographic information;
the OBD speeds under the navigation coordinate systems at the adjacent moments are respectively; at is the OBD speed update interval, i.e. the time difference between time k-1 and time k.
4. The navigation positioning method according to any one of claims 1-3, wherein the method for performing feedback compensation on the output information of the inertial sensor according to the processing result of Kalman filtering and obtaining the optimal position information comprises the steps of:
feedback correction of the sensor is realized according to the sensor error obtained by Kalman filtering;
and updating the Kalman filter parameters for the next filtering.
5. A navigation positioning system is characterized by comprising an inertial sensor, a mileage recorder or an OBD vehicle-mounted diagnosis system, a position information acquisition module, an information calculation module, an optimal position and error estimation module and a feedback compensation correction module;
the inertial sensor, the mileage recorder or the vehicle-mounted diagnosis system are respectively connected with a position information acquisition module, the position information acquisition module is connected with an information calculation module, the information calculation module is connected with an optimal position and error estimation module, and the optimal position and error estimation module is connected with a feedback compensation correction module;
the position information acquisition module is used for completing inertial navigation information calculation by using information sensed by the inertial sensor to acquire position information;
the information calculation module is used for calculating speed and position information by using the mileage speed acquired by a mileage meter or an OBD through the attitude information calculated by the inertial sensor;
the optimal position and error estimation module is used for realizing navigation information fusion processing by adopting a Kalman filter based on position information calculated by an inertial sensor and position information calculated by an OBD vehicle speed to obtain optimal position information and sensor error estimation;
the feedback compensation correction module is used for performing feedback compensation correction on the inertial navigation output information according to a Kalman filtering processing result to acquire optimal position information;
the optimal position and error estimation module includes a time update module and a measurement update module,
the time updating module is used for passing through the speed and position information calculated by the inertial sensor when the OBD vehicle speed information does not complete time updating
Completing time updating;
the measurement updating module is used for updating the OBD speed information at the moment, calculating the position information according to the OBD speed and the position information calculated by the inertial sensor, and passing through
The measurement update is completed, and the measurement update is completed,
wherein the measurement information
An error indicating that the OBD estimated position and the inertial sensor estimated position are at the same time; x k State quantities including attitude angle error, velocity error, position error and sensor bias error; h k Is a measurement matrix; phi is a state transition matrix; k k Is a gain matrix; f is a system structure parameter; wherein the subscript k represents the time k, and k/k-1 represents the prediction of the time k value at the time k-1;
p is position prediction information, P K/K-1 Position prediction information representing the value at time k-1 versus time k,
R K a variance matrix representing the state quantities;
Q k-1 representing an observation variance matrix at time k-1;
i denotes an identity matrix.
6. The system according to claim 5, wherein the position information acquiring module comprises a horizontal initial attitude information acquiring unit, an initial position and speed acquiring unit, an attitude matrix initialization calculating unit and an inertial navigation calculating unit;
the horizontal initial attitude information acquisition unit is used for acquiring horizontal acceleration data of the vehicle within a period of time through the accelerometer after the vehicle is started, and calculating a roll angle and a pitch angle by using the horizontal acceleration data as horizontal initial attitude information of inertial navigation;
the initial position and speed acquisition unit is used for adopting the position and speed of the GPS as the initial position and the initial speed of the inertial navigation;
the attitude matrix initialization calculation unit is used for completing initialization of the inertial navigation course angle information by utilizing the course angle information of the GPS and completing the attitude matrix by combining the horizontal attitude angle
The initialization calculation of (2);
the inertial navigation computing unit is used for performing inertial navigation computation by using information acquired by the inertial sensor at the current moment, and comprises an attitude matrix
And (4) updating and calculating the speed and the position.
7. A computer-readable storage medium, comprising a processor, a computer-readable storage medium, and a computer program stored on the computer-readable storage medium, which computer program, when executed by the processor, performs the steps of the method according to any one of claims 1 to 4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110515730.5A CN113252048B (en) | 2021-05-12 | 2021-05-12 | Navigation positioning method, navigation positioning system and computer readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110515730.5A CN113252048B (en) | 2021-05-12 | 2021-05-12 | Navigation positioning method, navigation positioning system and computer readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113252048A CN113252048A (en) | 2021-08-13 |
| CN113252048B true CN113252048B (en) | 2023-02-28 |
Family
ID=77222922
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110515730.5A Active CN113252048B (en) | 2021-05-12 | 2021-05-12 | Navigation positioning method, navigation positioning system and computer readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113252048B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113873637A (en) * | 2021-10-26 | 2021-12-31 | 上海瑾盛通信科技有限公司 | Positioning method, positioning device, terminal and storage medium |
| CN114413898B (en) * | 2022-03-29 | 2022-07-29 | 深圳市边界智控科技有限公司 | Multi-sensor data fusion method and device, computer equipment and storage medium |
| CN117405145B (en) * | 2023-12-14 | 2024-02-20 | 深圳市晟丰达科技有限公司 | Inertial navigation management method, system and storage medium based on intelligent analysis |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106950586A (en) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN109974697A (en) * | 2019-03-21 | 2019-07-05 | 中国船舶重工集团公司第七0七研究所 | A kind of high-precision mapping method based on inertia system |
| CN110221333A (en) * | 2019-04-11 | 2019-09-10 | 同济大学 | A kind of error in measurement compensation method of vehicle-mounted INS/OD integrated navigation system |
| CN110779521A (en) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | Multi-source fusion high-precision positioning method and device |
| CN111562603A (en) * | 2020-06-30 | 2020-08-21 | 高新兴物联科技有限公司 | Navigation positioning method, equipment and storage medium based on dead reckoning |
| CN112066983A (en) * | 2020-09-08 | 2020-12-11 | 中国人民解放军国防科技大学 | Inertial/odometer combined navigation filtering method, electronic equipment and storage medium |
| CN112762961A (en) * | 2020-12-28 | 2021-05-07 | 厦门华源嘉航科技有限公司 | On-line calibration method for integrated navigation of vehicle-mounted inertial odometer |
-
2021
- 2021-05-12 CN CN202110515730.5A patent/CN113252048B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106950586A (en) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN109974697A (en) * | 2019-03-21 | 2019-07-05 | 中国船舶重工集团公司第七0七研究所 | A kind of high-precision mapping method based on inertia system |
| CN110221333A (en) * | 2019-04-11 | 2019-09-10 | 同济大学 | A kind of error in measurement compensation method of vehicle-mounted INS/OD integrated navigation system |
| CN110779521A (en) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | Multi-source fusion high-precision positioning method and device |
| CN111562603A (en) * | 2020-06-30 | 2020-08-21 | 高新兴物联科技有限公司 | Navigation positioning method, equipment and storage medium based on dead reckoning |
| CN112066983A (en) * | 2020-09-08 | 2020-12-11 | 中国人民解放军国防科技大学 | Inertial/odometer combined navigation filtering method, electronic equipment and storage medium |
| CN112762961A (en) * | 2020-12-28 | 2021-05-07 | 厦门华源嘉航科技有限公司 | On-line calibration method for integrated navigation of vehicle-mounted inertial odometer |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113252048A (en) | 2021-08-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113252048B (en) | Navigation positioning method, navigation positioning system and computer readable storage medium | |
| US10240931B2 (en) | System and method for navigation by applying corrected bias values to gyroscopic data | |
| US9488480B2 (en) | Method and apparatus for improved navigation of a moving platform | |
| EP2095148B1 (en) | Arrangement for and method of two dimensional and three dimensional precision location and orientation determination | |
| CN114545472B (en) | Navigation method and device of GNSS/INS combined system | |
| CN110715659A (en) | Zero-speed detection method, pedestrian inertial navigation method, device and storage medium | |
| JP6149699B2 (en) | Vehicle trajectory calculation device | |
| CN112835085B (en) | Method and device for determining vehicle position | |
| CN114413934B (en) | Correction method and device for vehicle positioning system | |
| JP6248559B2 (en) | Vehicle trajectory calculation device | |
| KR100948089B1 (en) | Method for deciding car position by pseudo dead reckoning and car navigation system using the same | |
| CN115060257B (en) | Vehicle lane change detection method based on civil-grade inertia measurement unit | |
| CN114234969B (en) | Navigation positioning method and device and electronic equipment | |
| CN115407376A (en) | Vehicle positioning calibration method and device, computer equipment and storage medium | |
| US20220057517A1 (en) | Method for constructing point cloud map, computer device, and storage medium | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN114061570A (en) | Vehicle positioning method and device, computer equipment and storage medium | |
| CN114019954B (en) | Course installation angle calibration method, device, computer equipment and storage medium | |
| CN114001730B (en) | Fusion positioning method, fusion positioning device, computer equipment and storage medium | |
| CN115112119A (en) | Vehicle navigation method based on LSTM neural network assistance | |
| CN112611377A (en) | State prediction method and device for outdoor navigation of trolley and storage medium | |
| CN112835086A (en) | Method and device for determining vehicle position | |
| CN113566849B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN115343738A (en) | GNSS-RTK and IMU based integrated navigation method and equipment | |
| CN117053832A (en) | Odometer factor calibration method and device |
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 | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 518000 606, block a, sharing building, No. 78, Keyuan North Road, songpingshan community, Xili street, Nanshan District, Shenzhen, Guangdong Applicant after: Gosuncn IOT Technology Co.,Ltd. Address before: 518000 Guangdong, Shenzhen, Nanshan District, Guangdong Province, Nanshan District high tech Industrial Park, South Korea 12 road, MINDRAY Building 2 floor B area, zone 12 Applicant before: Gaoxing Wulian Technology Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |