CN115236708A - Method, device and equipment for estimating position and attitude state of vehicle and storage medium - Google Patents
Method, device and equipment for estimating position and attitude state of vehicle and storage medium Download PDFInfo
- Publication number
- CN115236708A CN115236708A CN202210878107.0A CN202210878107A CN115236708A CN 115236708 A CN115236708 A CN 115236708A CN 202210878107 A CN202210878107 A CN 202210878107A CN 115236708 A CN115236708 A CN 115236708A
- Authority
- CN
- China
- Prior art keywords
- data
- observation
- vehicle
- covariance
- state estimation
- 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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Mathematical Physics (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Software Systems (AREA)
- Algebra (AREA)
- Computer Networks & Wireless Communication (AREA)
- Databases & Information Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Life Sciences & Earth Sciences (AREA)
- Probability & Statistics with Applications (AREA)
- Evolutionary Biology (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Computing Systems (AREA)
- Navigation (AREA)
Abstract
The application relates to a method, a device, equipment and a storage medium for estimating a position and attitude state of a vehicle, wherein the method comprises the following steps: acquiring control data and observation data of a vehicle, processing the control data and the observation data, and storing the processed control data and the observation data into a pre-established data queue according to a data timestamp; constructing a mapping container based on a pre-established data queue, and judging whether data with the minimum data time stamp in the mapping container is observation data; if so, predicting and processing the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix of the vehicle at the previous moment, obtaining an optimal state estimation and a covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position posture state of the vehicle at the next moment. Therefore, the problem that positioning of a GNSS signal-free area is not considered in the related technology is solved, and the positioning accuracy of a blind area is improved.
Description
Technical Field
The present disclosure relates to the field of vehicle technologies, and in particular, to a method, an apparatus, a device, and a storage medium for estimating a position and an attitude of a vehicle.
Background
The automatic driving is a product of deep integration of the automobile industry and new generation information technologies such as artificial intelligence, internet of things, high-performance computing and the like, is a main direction of intelligent and networking development of the automobile and traffic travel field in the world at present, and has become a strategic high point of controversy of various countries.
As is well known, the first step of automatic driving is self-positioning of the vehicle, and only if the position of the vehicle is known, the downstream control and decision end can perform path planning to control the vehicle to run, and the robustness and safety of the automatic driving system are directly affected by the quality of the positioning accuracy.
In the related art, one of the methods is to closely combine an Inertial Measurement Unit (IMU) and a navigator (Global Navigation Satellite System, GNSS), and realize positioning and attitude determination in a high dynamic and strong interference environment by using the long-term stability of positioning and orientation accuracy of the GNSS navigator and the short-term stability of the IMU Inertial Measurement Unit, and when a GNSS signal is blocked or the quality of the GNSS signal is reduced, the positioning and attitude determination can be performed by the IMU Inertial Measurement Unit, so that the reliability, integrity and continuity of the positioning and attitude determination device can be effectively improved; another implementation implements GNSS/INS tight-coupled navigation.
However, in the related technologies, only a single GNSS is used as a kalman filter system for observation, an IMU or an INS (information network) is used for control, positioning of a GNSS dead zone is not considered, and in a driving scene, there are often areas that the GNSS cannot cover, such as tunnels, viaducts, and urban roads covered by trees, and if only the GNSS is used as a positioning kalman system for observation, a positioning error is large and even a serious traffic accident is caused.
Disclosure of Invention
The application provides a method and a device for estimating the position and attitude state of a vehicle, the vehicle and a storage medium, which are used for solving the problem that the positioning of a GNSS non-signal area is not considered in the related technology, so that a larger positioning error is caused, overcoming the influence caused by the shielding of a high-rise building, optimizing data flow, improving the operation rate and simultaneously improving the positioning precision.
An embodiment of a first aspect of the present application provides a method for estimating a position and orientation state of a vehicle, including the following steps: acquiring control data and observation data of a vehicle, processing the control data and the observation data, and storing the control data and the observation data into a pre-established data queue according to a data timestamp; constructing a mapping container based on the pre-established data queue, and judging whether the data with the minimum data time stamp in the mapping container is the observation data; if the data with the minimum data timestamp is the observation data, predicting the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix at the last moment of the vehicle; and based on the optimal state estimation and covariance matrix of the vehicle at the previous moment, obtaining the optimal state estimation and covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position and attitude state of the vehicle at the next moment according to the optimal state estimation and covariance matrix of the vehicle at the current moment.
According to the technical means, the problem that a large positioning error is caused because positioning of a GNSS non-signal area is not considered in the related technology can be solved, the influence caused by shielding of a high-rise building is overcome, data flow is optimized, the operation rate is improved, and meanwhile positioning accuracy is improved.
Optionally, in some embodiments, the processing the control data and the observation data includes: converting the triaxial linear velocity and the triaxial angular velocity of the vehicle under the inertial navigation system INS coordinate system combined in the control data to a preset coordinate system to obtain the INS noise covariance; converting first position information and first course information of the vehicle, which are obtained by a Global Navigation Satellite System (GNSS) in the observation data, into the INS coordinate system to obtain the noise covariance, converting second position information and second course information of the vehicle, which are obtained by the FC, into the INS coordinate system to obtain an FC (Functional Circuit, function test) observation covariance, and obtaining the observation noise covariance according to the noise covariance and the FC observation covariance.
According to the technical means, the system states of the position, the speed, the attitude and the like of the carrier can be solved by using the DE combined Inertial Navigation System (INS), and meanwhile, in order to directly utilize the INS motion equation to carry out integration and generate obvious drift along with time, the method fuses information (such as GNSS and FC) of other sensors and corrects the INS motion integration result.
Optionally, in some embodiments, the constructing a mapping container based on the pre-established data queue includes: receiving tail single-frame data in the pre-established data queue; and taking the data time stamp of the tail single-frame data as a key value, taking control data or observation data in the tail single-frame data as a value, and constructing the mapping container according to the key value and the value.
According to the technical means, the data center is established, the data flow is optimized, the Kalman iteration of invalid data is reduced, and the operation speed is improved.
Optionally, in some embodiments, after determining whether the data with the smallest data timestamp in the mapping container is the observation data, the method further includes: and if the data is the observation data, initializing the state position of the data in the mapping container.
According to the technical means, the data center is established, the data flow is optimized, the Kalman iteration of invalid data is reduced, and the operation speed is improved.
Optionally, in some embodiments, the equation of motion is:
the observation equation is:
y k ＝g(x k )+v k ；
wherein x is k For optimal estimation of time k, k being time, u k For controlling quantities by equations of motion, w k To controlNoise, v k To observe the noise.
According to the technical means, the positioning precision of the GNSS is improved by using an extended Kalman filtering algorithm in the embodiment of the application; the positioning accuracy of the blind area is improved by utilizing a multi-sensor combined positioning algorithm, and the influence caused by shielding of a high-rise building is overcome; by establishing the data center, the data flow is optimized, the invalid data Kalman iteration is reduced, and the operation rate is improved.
An embodiment of a second aspect of the present application provides a position and orientation state estimation device for a vehicle, including: the system comprises an acquisition module, a data queue and a data processing module, wherein the acquisition module is used for acquiring control data and observation data of a vehicle, processing the control data and the observation data and storing the processed control data and the observation data into the data queue which is established in advance according to a data timestamp; the judging module is used for constructing a mapping container based on the pre-established data queue and judging whether the data with the minimum data time stamp in the mapping container is the observation data or not; the processing module is used for predicting and processing the data with the minimum data timestamp by combining the control noise covariance in the control data and the observation noise covariance in the observation data based on a preset motion equation and an observation equation to obtain the optimal state estimation and covariance matrix at the last moment of the vehicle if the data with the minimum data timestamp is the observation data; and the state estimation module is used for obtaining the optimal state estimation and the covariance matrix of the vehicle at the current moment according to a Kalman filtering equation based on the optimal state estimation and the covariance matrix at the previous moment of the vehicle, and estimating the position and attitude state of the vehicle at the next moment according to the optimal state estimation and the covariance matrix of the vehicle at the current moment.
Optionally, in some embodiments, the obtaining module is specifically configured to: converting the triaxial linear velocity and the triaxial angular velocity of the vehicle under the inertial navigation system INS coordinate system combined in the control data to a preset coordinate system to obtain the INS noise covariance; converting first position information and first course information of the vehicle, which are obtained by a Global Navigation Satellite System (GNSS) in the observation data, into the INS coordinate system to obtain the noise covariance, converting second position information and second course information of the vehicle, which are obtained by the FC, into the INS coordinate system to obtain an FC observation covariance, and obtaining the observation noise covariance according to the noise covariance and the FC observation covariance.
Optionally, in some embodiments, the determining module is specifically configured to: receiving tail single-frame data in the pre-established data queue; and taking the data time stamp of the tail single-frame data as a key value, taking control data or observation data in the tail single-frame data as a value, and constructing the mapping container according to the key value and the value.
Optionally, in some embodiments, after determining whether the data with the smallest data timestamp in the mapping container is the observation data, the determining module is further configured to: and if the data is the observation data, initializing the state position of the data in the mapping container.
Optionally, in some embodiments, the equation of motion is:
the observation equation is:
y k ＝g(x k )+v k ；
wherein x is k For optimal estimation of time k, k being time, u k For controlling quantities by equations of motion, w k To control noise, v k To observe the noise.
An embodiment of a third aspect of the present application provides a vehicle, comprising: a memory, a processor and a computer program stored on the memory and executable on the processor, the processor executing the program to implement the position posture state estimation method of the vehicle as described in the above embodiments.
A fourth aspect embodiment of the present application provides a computer-readable storage medium having stored thereon a computer program, which is executed by a processor, for implementing the position and orientation state estimation method of a vehicle as described in the above embodiments.
Therefore, control data and observation data of the vehicle are acquired, processed and stored in a pre-established data queue according to the data time stamp; constructing a mapping container based on a pre-established data queue, and judging whether data with the minimum data time stamp in the mapping container is observation data; if so, predicting and processing the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix of the vehicle at the previous moment, obtaining an optimal state estimation and a covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position posture state of the vehicle at the next moment. Therefore, the problem of large positioning error caused by the fact that positioning of a GNSS signal-free area is not considered in the related art is solved, the influence caused by shielding of a high-rise building is overcome, data flow is optimized, the operation rate is improved, and meanwhile positioning accuracy is improved.
Additional aspects and advantages of the present application will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the present application.
Drawings
The foregoing and/or additional aspects and advantages of the present application will become apparent and readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings of which:
fig. 1 is a flowchart of a method for estimating a position and orientation state of a vehicle according to an embodiment of the present application;
FIG. 2 is a schematic diagram of a data center provided in accordance with one embodiment of the present application;
FIG. 3 is a schematic diagram of a state estimation module provided in accordance with an embodiment of the present application;
FIG. 4 is a schematic diagram of a method for estimating a position and orientation state of a vehicle according to an embodiment of the present application;
fig. 5 is a block schematic diagram of a position and orientation state estimation device of a vehicle according to an embodiment of the present application;
fig. 6 is a schematic view of an electronic device provided according to an embodiment of the present application.
Description of the reference numerals: 10-position and attitude state estimation device of vehicle, 100-acquisition module, 2000-judgment module, 300-processing module and 400-state updating module.
Detailed Description
Reference will now be made in detail to embodiments of the present application, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are exemplary and intended to be used for explaining the present application and should not be construed as limiting the present application.
A position and orientation state estimation method of a vehicle, an apparatus, a vehicle, and a storage medium according to embodiments of the present application are described below with reference to the drawings. In order to solve the problem that a large positioning error is caused by positioning of a GNSS non-signal area which is not considered in the background art, the application provides a method for estimating the position and attitude state of a vehicle, wherein in the method, control data and observation data of the vehicle are obtained and are stored into a pre-established data queue according to a data time stamp after being processed; constructing a mapping container based on a pre-established data queue, and judging whether data with the minimum data time stamp in the mapping container is observation data; if so, predicting and processing the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix of the vehicle at the previous moment, obtaining an optimal state estimation and a covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position posture state of the vehicle at the next moment. Therefore, the problem of large positioning error caused by positioning of a GNSS non-signal area is not considered in the related technology is solved, the influence caused by shielding of a high-rise building is overcome, data flow is optimized, the operation rate is improved, and meanwhile, the positioning precision is improved.
Specifically, fig. 1 is a schematic flowchart of a method for estimating a position and orientation state of a vehicle according to an embodiment of the present application.
As shown in fig. 1, the position posture state estimation method of the vehicle includes the steps of:
in step S101, control data and observation data of the vehicle are acquired, processed, and then stored in a data queue established in advance according to a data timestamp.
Optionally, in some embodiments, the processing the control data and the observation data comprises: converting the triaxial linear velocity and the triaxial angular velocity of the vehicle under the combined inertial navigation system INS coordinate system in the control data to a preset coordinate system to obtain an INS noise covariance; converting first position information and first course information of the vehicle, which are obtained by a Global Navigation Satellite System (GNSS) in observation data, into an INS coordinate system to obtain noise covariance, converting second position information and second course information of the vehicle, which are obtained by the FC, into the INS coordinate system to obtain FC observation covariance, and obtaining observation noise covariance according to the noise covariance and the FC observation covariance.
Specifically, in the embodiment of the present application, the data processing is performed by a data center module, as shown in fig. 2, including the following steps:
the method comprises the following steps: preprocessing control data, namely using INS output data as control quantity, wherein the control quantity comprises a formula obtained by converting triaxial linear velocity and triaxial angular velocity under an INS coordinate system into a local coordinate system to obtain an INS noise covariance Q;
step two: preprocessing observation data, namely, taking data output by a GNSS as an observation quantity, and converting the position information and the course information contained in the observation quantity into an INS coordinate system to be expressed as a formula to obtain a GNSS noise covariance Rg; converting the position information and the attitude information observed by the FC into an INS coordinate system formula to obtain FC observation covariance Rf;
step three: constructing a data queue container, realizing the first-in first-out of the inserted data, and sequencing according to the data time stamp;
step four: and putting the observation and control data into a data queue.
In step S102, a mapping container is constructed based on a pre-established data queue, and it is determined whether data with the smallest data timestamp in the mapping container is observation data.
Optionally, in some embodiments, constructing the mapping container based on a pre-established data queue includes: receiving tail single-frame data in a pre-established data queue; and taking the data time stamp of the single frame data of the tail part as a key value, taking control data or observation data in the single frame data of the tail part as a value, and constructing a mapping container according to the key value and the value.
Optionally, in some embodiments, after determining whether the data with the smallest data timestamp in the mapping container is the observation data, the method further includes: if the data is the observation data, the state position of the data in the mapping container is initialized.
Specifically, as shown in fig. 3, in the embodiment of the present application, a mapping container B is first constructed, tail single-frame data in a data queue is received, a data timestamp is a key value, and control type data or observation type data is a value; and taking the data with the earliest time stamp in the container B, judging whether the data type is the observation type, finishing the initialization of the state position, skipping to execute the step S101 if the data is not the observation type, and executing the step S103 if the data is the observation type.
In step S103, if the data with the minimum data timestamp is the observation data, the data with the minimum data timestamp is predicted based on a preset motion equation and an observation equation, and by combining the control noise covariance in the control data and the observation noise covariance in the observation data, the optimal state estimation and covariance matrix at the last time on the vehicle are obtained.
Specifically, in the embodiment of the present application, the above-mentioned data control noise covariance Q and observation noise covariance R are combined to make the optimal state estimation at the given time k-1 for prediction processing
And
the covariance matrix is then used to determine the covariance matrix,
optionally, in some embodiments, the equation of motion is:
the observation equation is:
y k ＝g(x k )+v k ；
wherein x is k For optimal estimation of time k, k being time, u k For controlling quantities by equations of motion, w k To control noise, a Gaussian distribution, v, is fit k To observe noise, a gaussian distribution is conformed.
The optimal estimation of the state at the previous moment is as follows:
the covariance matrix at the previous time is:
in step S104, based on the optimal state estimation and the covariance matrix at the previous time of the vehicle, the optimal state estimation and the covariance matrix at the current time of the vehicle are obtained according to the kalman filter equation, and the position and orientation state of the vehicle at the next time is estimated according to the optimal state estimation and the covariance matrix at the current time of the vehicle.
The kalman filter used in the embodiment of the present application is the most widely used one of the variations of the Extended Kalman Filter (EKF), and is a nonlinear filter for a time varying system, as with the EKF, but unlike the EKF, the linearization is always around 0, and thus the linearization is more accurate.
Specifically, in the embodiment of the present application, the optimal state estimation and the covariance matrix Pk at the time k are calculated according to the extended kalman filter equation. The state optimal estimate and covariance matrix use a recursive algorithm, by which the state optimal estimate Xk and covariance matrix PK at the next time instant k can be determined, given that the state optimal estimate and covariance matrix at time instant k-1 are known. And circularly traversing the data in the mapping container B to complete Kalman filtering prediction and updating of each frame of data. And outputting the state estimation value, and returning to the step S101 of the state estimation module.
Calculating a Kalman gain:
updating the optimal estimate for the estimated time k by the observed variable y:
error covariance at time K:
therefore, as shown in fig. 3, in the embodiment of the present application, an extended kalman filter algorithm is used to improve the positioning accuracy of the GNSS; the positioning accuracy of the blind area is improved by utilizing a multi-sensor combined positioning algorithm, and the influence caused by shielding of a high-rise building is overcome; by establishing a data center, data flow is optimized, invalid data Kalman iteration is reduced, and the operation rate is improved; in addition, the GNSS/FC/IMU is used for combined positioning, and when the GNSS signal is unlocked, the positioning information of the IMU and the FC is subjected to extended Kalman filtering, so that more accurate positioning is output.
According to the method for estimating the position and attitude state of the vehicle, control data and observation data of the vehicle are obtained and are stored into a pre-established data queue according to a data timestamp after being processed; constructing a mapping container based on a pre-established data queue, and judging whether data with the minimum data time stamp in the mapping container is observation data; if so, predicting and processing the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix of the vehicle at the previous moment, obtaining an optimal state estimation and a covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position posture state of the vehicle at the next moment. Therefore, the problem of large positioning error caused by positioning of a GNSS non-signal area is not considered in the related technology is solved, the influence caused by shielding of a high-rise building is overcome, data flow is optimized, the operation rate is improved, and meanwhile, the positioning precision is improved.
Next, a position and orientation state estimation device of a vehicle proposed according to an embodiment of the present application is described with reference to the drawings.
Fig. 5 is a block schematic diagram of a position posture state estimation device of a vehicle according to an embodiment of the present application.
As shown in fig. 5, the position and orientation state estimation device 10 for a vehicle includes: an acquisition module 100, a determination module 200, a processing module 300, and a status update module 400.
The system comprises an acquisition module 100, a data queue and a data processing module, wherein the acquisition module is used for acquiring control data and observation data of a vehicle, processing the control data and the observation data and storing the control data and the observation data into the data queue established in advance according to a data timestamp; the judging module 200 is used for constructing a mapping container based on a pre-established data queue and judging whether data with the minimum data time stamp in the mapping container is observation data or not; the processing module 300 is configured to, if the data with the smallest data timestamp is observation data, predict and process the data with the smallest data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix at the last moment of the vehicle; and a state estimation module 400, which obtains the optimal state estimation and the covariance matrix of the vehicle at the current moment according to the kalman filter equation based on the optimal state estimation and the covariance matrix at the previous moment of the vehicle, and estimates the position and attitude state of the vehicle at the next moment according to the optimal state estimation and the covariance matrix of the vehicle at the current moment.
Optionally, in some embodiments, the obtaining module 100 is specifically configured to: converting the triaxial linear velocity and the triaxial angular velocity of the vehicle under the combined inertial navigation system INS coordinate system in the control data to a preset coordinate system to obtain an INS noise covariance; converting first position information and first course information of the vehicle, which are obtained by a Global Navigation Satellite System (GNSS) in observation data, into an Inertial Navigation System (INS) coordinate system to obtain noise covariance, converting second position information and second course information of the vehicle, which are obtained by a computer controller (FC), into the INS coordinate system to obtain FC observation covariance, and obtaining observation noise covariance according to the noise covariance and the FC observation covariance.
Optionally, in some embodiments, the determining module 200 is specifically configured to: receiving tail single-frame data in a pre-established data queue; and taking the data time stamp of the single frame data of the tail part as a key value, taking control data or observation data in the single frame data of the tail part as a value, and constructing a mapping container according to the key value and the value.
Optionally, in some embodiments, after determining whether the data with the smallest data timestamp in the mapping container is the observation data, the determining module 200 is further configured to: if the data is the observation data, the state position of the data in the mapping container is initialized.
Optionally, in some embodiments, the equation of motion is:
the observation equation is:
y k ＝g(x k )+v k ；
wherein x is k For optimal estimation of time k, k being time, u k For controlling quantities by equations of motion, w k For noise control, fitting a Gaussian distribution, v k To observe the noise, a gaussian distribution is conformed.
It should be noted that the foregoing explanation of the embodiment of the method for estimating a position and orientation state of a vehicle also applies to the device for estimating a position and orientation state of a vehicle of this embodiment, and is not repeated here.
According to the position and posture state estimation device for the vehicle, control data and observation data of the vehicle are obtained and are stored into a pre-established data queue according to a data timestamp after being processed; constructing a mapping container based on a pre-established data queue, and judging whether data with the minimum data time stamp in the mapping container is observation data; if so, predicting and processing the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix of the vehicle at the previous moment, obtaining an optimal state estimation and a covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position posture state of the vehicle at the next moment. Therefore, the problem of large positioning error caused by positioning of a GNSS non-signal area is not considered in the related technology is solved, the influence caused by shielding of a high-rise building is overcome, data flow is optimized, the operation rate is improved, and meanwhile, the positioning precision is improved.
Fig. 6 is a schematic structural diagram of a vehicle according to an embodiment of the present application. The vehicle may include:
a memory 601, a processor 602, and a computer program stored on the memory 601 and executable on the processor 602.
The processor 602 implements the position posture state estimation method of the vehicle provided in the above-described embodiment when executing the program.
Further, the vehicle further includes:
a communication interface 603 for communication between the memory 601 and the processor 602.
The memory 601 is used for storing computer programs that can be run on the processor 602.
The Memory 601 may include a high-speed RAM (Random Access Memory) Memory, and may also include a non-volatile Memory, such as at least one disk Memory.
If the memory 601, the processor 602 and the communication interface 603 are implemented independently, the communication interface 603, the memory 601 and the processor 602 may be connected to each other through a bus and perform communication with each other. The bus may be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component interconnect) bus, an EISA (Extended Industry Standard Architecture) bus, or the like. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one thick line is shown in FIG. 6, but that does not indicate only one bus or one type of bus.
Optionally, in a specific implementation, if the memory 601, the processor 602, and the communication interface 603 are integrated on a chip, the memory 601, the processor 602, and the communication interface 603 may complete mutual communication through an internal interface.
The processor 602 may be a Central Processing Unit (CPU), an Application Specific Integrated Circuit (ASIC), or one or more Integrated circuits configured to implement embodiments of the present Application.
Embodiments of the present application also provide a computer-readable storage medium on which a computer program is stored, which when executed by a processor, implements the position and orientation state estimation method of a vehicle as above.
In the description of the present specification, reference to the description of "one embodiment," "some embodiments," "an example," "a specific example," or "some examples" or the like means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the present application. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or N embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present application, "N" means at least two, e.g., two, three, etc., unless specifically limited otherwise.
Any process or method descriptions in flow charts or otherwise described herein may be understood as representing modules, segments, or portions of code which include one or more N executable instructions for implementing steps of a custom logic function or process, and alternate implementations are included within the scope of the preferred embodiment of the present application in which functions may be executed out of order from that shown or discussed, including substantially concurrently or in reverse order, depending on the functionality involved, as would be understood by those reasonably skilled in the art of implementing the embodiments of the present application.
It should be understood that portions of the present application may be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods may be implemented in software or firmware stored in a memory and executed by a suitable instruction execution system. If implemented in hardware, as in another embodiment, any one or combination of the following techniques, which are known in the art, may be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application specific integrated circuit having an appropriate combinational logic gate circuit, a programmable gate array, a field programmable gate array, or the like.
It will be understood by those skilled in the art that all or part of the steps carried by the method for implementing the above embodiments may be implemented by hardware related to instructions of a program, which may be stored in a computer readable storage medium, and when the program is executed, the program includes one or a combination of the steps of the method embodiments.
Although embodiments of the present application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present application, and that variations, modifications, substitutions and alterations may be made to the above embodiments by those of ordinary skill in the art within the scope of the present application.
Claims (10)
1. A position and orientation state estimation method of a vehicle, characterized by comprising the steps of:
acquiring control data and observation data of a vehicle, processing the control data and the observation data, and storing the processed control data and the observation data into a pre-established data queue according to a data timestamp;
constructing a mapping container based on the pre-established data queue, and judging whether the data with the minimum data time stamp in the mapping container is the observation data;
if the data with the minimum data timestamp is the observation data, predicting the data with the minimum data timestamp based on a preset motion equation and an observation equation by combining a control noise covariance in the control data and an observation noise covariance in the observation data to obtain an optimal state estimation and a covariance matrix at the last moment of the vehicle; and
and based on the optimal state estimation and covariance matrix of the vehicle at the previous moment, obtaining the optimal state estimation and covariance matrix of the vehicle at the current moment according to a Kalman filtering equation, and estimating the position and attitude state of the vehicle at the next moment according to the optimal state estimation and covariance matrix of the vehicle at the current moment.
2. The method of claim 1, wherein the processing the control data and the observation data comprises:
converting the triaxial linear velocity and the triaxial angular velocity of the vehicle under the inertial navigation system INS coordinate system combined in the control data to a preset coordinate system to obtain the INS noise covariance;
converting first position information and first course information of the vehicle, which are obtained by a Global Navigation Satellite System (GNSS) in the observation data, into the INS coordinate system to obtain the noise covariance, converting second position information and second course information of the vehicle, which are obtained by the FC, into the INS coordinate system to obtain an FC observation covariance, and obtaining the observation noise covariance according to the noise covariance and the FC observation covariance.
3. The method of claim 1, wherein constructing a mapping container based on the pre-established data queue comprises:
receiving tail single-frame data in the pre-established data queue;
and taking the data time stamp of the tail single-frame data as a key value, taking control data or observation data in the tail single-frame data as a value, and constructing the mapping container according to the key value and the value.
4. The method of claim 1, after determining whether the data with the smallest data timestamp in the mapping container is the observation data, further comprising:
and if the data is the observation data, initializing the state position of the data in the mapping container.
5. The method according to any one of claims 1-4, wherein the equation of motion is:
the observation equation is:
y k ＝g(x k )+v k ；
wherein x is k For a priori estimation of time k, k being time u k For controlling quantities by equations of motion, w k To control noise, v k To observe the noise.
6. A position and orientation state estimation device of a vehicle, characterized by comprising:
the system comprises an acquisition module, a data queue and a data processing module, wherein the acquisition module is used for acquiring control data and observation data of a vehicle, processing the control data and the observation data and storing the processed control data and the observation data into the data queue which is established in advance according to a data timestamp;
the judging module is used for constructing a mapping container based on the pre-established data queue and judging whether the data with the minimum data time stamp in the mapping container is the observation data or not;
the processing module is used for predicting and processing the data with the minimum data timestamp by combining the control noise covariance in the control data and the observation noise covariance in the observation data based on a preset motion equation and an observation equation to obtain the optimal state estimation and covariance matrix at the last moment of the vehicle if the data with the minimum data timestamp is the observation data; and
and the state estimation module is used for obtaining the optimal state estimation and the covariance matrix of the vehicle at the current moment according to a Kalman filtering equation based on the optimal state estimation and the covariance matrix at the previous moment of the vehicle, and estimating the position and attitude state of the vehicle at the next moment according to the optimal state estimation and the covariance matrix of the vehicle at the current moment.
7. The apparatus of claim 6, wherein the obtaining module is specifically configured to:
converting the triaxial linear velocity and the triaxial angular velocity of the vehicle under the inertial navigation system INS coordinate system combined in the control data to a preset coordinate system to obtain the INS noise covariance;
converting first position information and first course information of the vehicle, which are obtained by a Global Navigation Satellite System (GNSS) in the observation data, into the INS coordinate system to obtain the noise covariance, converting second position information and second course information of the vehicle, which are obtained by the FC, into the INS coordinate system to obtain an FC observation covariance, and obtaining the observation noise covariance according to the noise covariance and the FC observation covariance.
8. The apparatus of claim 6, wherein the determining module is specifically configured to:
receiving tail single-frame data in the pre-established data queue;
and taking the data time stamp of the tail single-frame data as a key value, taking the control data or the observation data in the tail single-frame data as a value, and constructing the mapping container according to the key value and the value.
9. An electronic device, comprising: a memory, a processor and a computer program stored on the memory and executable on the processor, the processor executing the program to implement the position posture state estimation method of the vehicle according to any one of claims 1 to 5.
10. A computer-readable storage medium having stored thereon a computer program, characterized in that the program is executed by a processor for implementing a position posture state estimation method of a vehicle according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210878107.0A CN115236708A (en) | 2022-07-25 | 2022-07-25 | Method, device and equipment for estimating position and attitude state of vehicle and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210878107.0A CN115236708A (en) | 2022-07-25 | 2022-07-25 | Method, device and equipment for estimating position and attitude state of vehicle and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115236708A true CN115236708A (en) | 2022-10-25 |
Family
ID=83675808
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210878107.0A Pending CN115236708A (en) | 2022-07-25 | 2022-07-25 | Method, device and equipment for estimating position and attitude state of vehicle and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115236708A (en) |
-
2022
- 2022-07-25 CN CN202210878107.0A patent/CN115236708A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20200216076A1 (en) | Method for determining the location of an ego-vehicle | |
| Jo et al. | GPS-bias correction for precise localization of autonomous vehicles | |
| EP2664894B1 (en) | Navigation apparatus | |
| CN106558219B (en) | Vehicle track prediction method and device | |
| CN108700423B (en) | In-vehicle device and estimation method | |
| US9057615B2 (en) | Systems and methods for navigating using corrected yaw bias values | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| CN113405545B (en) | Positioning method, positioning device, electronic equipment and computer storage medium | |
| US20150153460A1 (en) | Sequential Estimation in a Real-Time Positioning or Navigation System Using Historical States | |
| CN114264301B (en) | Vehicle-mounted multi-sensor fusion positioning method, device, chip and terminal | |
| WO2012137415A1 (en) | Position calculating method and position calculating device | |
| KR20130068399A (en) | Apparatus and method for error compensation for radar system | |
| CN114545472B (en) | Navigation method and device of GNSS/INS combined system | |
| JP5164645B2 (en) | Method and apparatus for repetitive calculation control in Kalman filter processing | |
| KR101141984B1 (en) | DR/GPS Data Fusion Method | |
| CN114915913A (en) | UWB-IMU combined indoor positioning method based on sliding window factor graph | |
| CN114063622A (en) | Unmanned ship autonomous berthing positioning method and device and related components | |
| CN106886037B (en) | POS data method for correcting error suitable for weak GNSS signal condition | |
| Emami et al. | A customized H-infinity algorithm for underwater navigation system: With experimental evaluation | |
| Park | Optimal vehicle position estimation using adaptive unscented Kalman filter based on sensor fusion | |
| CN110375740B (en) | Vehicle navigation method, device, equipment and storage medium | |
| CN115236708A (en) | Method, device and equipment for estimating position and attitude state of vehicle and storage medium | |
| CN114001730B (en) | Fusion positioning method, fusion positioning device, computer equipment and storage medium | |
| JP6962040B2 (en) | Direction estimation device | |
| CN111123323B (en) | Method for improving positioning precision of portable equipment |
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 |