CN114063110A - Vehicle positioning method and device, electronic equipment and storage medium - Google Patents
Vehicle positioning method and device, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN114063110A CN114063110A CN202111509693.3A CN202111509693A CN114063110A CN 114063110 A CN114063110 A CN 114063110A CN 202111509693 A CN202111509693 A CN 202111509693A CN 114063110 A CN114063110 A CN 114063110A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- information
- positioning
- determining
- return signal
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/481—Constructional features, e.g. arrangements of optical elements
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
The application provides a vehicle positioning method, a vehicle positioning device, electronic equipment and a storage medium, wherein the method comprises the following steps: acquiring a first return signal received by a laser radar; determining the actual measurement position of the vehicle at the current positioning moment according to the first return signal; predicting the predicted position of the vehicle at the current positioning moment according to an inertial navigation system of the vehicle; and fusing the actual measurement position and the predicted position to obtain the current positioning information of the vehicle. In the embodiment, the error correction is performed on the final positioning result by utilizing the actual measurement position and the predicted position, so that the vehicle positioning precision is improved.
Description
Technical Field
The present disclosure relates to the field of positioning technologies, and in particular, to a vehicle positioning method and apparatus, an electronic device, and a storage medium.
Background
With the rise of automatic driving, the demand for perception of the driving environment is becoming more and more strict. In the automatic driving process of the vehicle, the laser radar looks like the eyes of the vehicle. Mechanical lidar is widely used in automatic driving vehicle testing, but the equipment cost is too high, the productivity is low, and the requirements of low cost and mass production of vehicle-mounted lidar are difficult to achieve.
In the related art, the MEMS solid-state lidar is applied to an autonomous vehicle, which has higher resolution, safety, and lower cost. However, the MEMS lidar And the inertial system usually operate in a synchronous positioning And Mapping (SLAM) manner, And the MEMS solid-state lidar often has poor zero drift And poor measurement accuracy, resulting in poor positioning accuracy.
Disclosure of Invention
An embodiment of the application aims to provide a vehicle positioning method, a vehicle positioning device, electronic equipment and a storage medium, and aims to solve the problem that the positioning accuracy of the current vehicle positioning result is poor.
In a first aspect, an embodiment of the present application provides a vehicle positioning method, including:
acquiring a first return signal received by a laser radar;
determining the actual measurement position of the vehicle at the current positioning moment according to the first return signal;
predicting the predicted position of the vehicle at the current positioning moment according to an inertial navigation system of the vehicle;
and fusing the actual measurement position and the predicted position to obtain the current positioning information of the vehicle.
In the embodiment, the actual measurement position of the vehicle at the current positioning moment is determined by acquiring the first return signal received by the laser radar and according to the first return signal, so that the laser radar and the vehicle-mounted radar are mutually compensated, and the accuracy of the measurement result of the laser radar is improved; and then, according to an inertial navigation system of the vehicle, predicting the predicted position of the vehicle at the current positioning moment, and fusing the actual measurement position and the predicted position to obtain the current positioning information of the vehicle, so that the actual measurement position and the predicted position are utilized to correct the error of the final positioning result, and the positioning precision of the vehicle is improved.
In one embodiment, determining the measured position of the vehicle at the current location time comprises:
determining first distance information of the vehicle and surrounding objects according to the first return signal;
acquiring second distance information between the vehicle and surrounding objects at the last positioning moment and last positioning information of the vehicle at the last positioning moment;
and determining the actual measurement position of the vehicle at the current positioning moment according to the first distance information, the second distance information and the last positioning information based on a preset Kalman filter.
In the embodiment, the first distance information between the vehicle and the surrounding object is determined through the first return signal, and then the actual measurement position of the vehicle at the current positioning time is measured by combining and utilizing the second distance information and the previous positioning information at the previous positioning time, so that the positioning accuracy is improved.
In one embodiment, the method further comprises:
acquiring a second return signal received by the vehicle-mounted radar;
determining a measured position of the vehicle at a current positioning time, comprising: and determining the measured position of the vehicle at the current positioning moment according to the second return signal.
In one embodiment, determining first distance information of the vehicle from surrounding objects comprises:
first distance information of the vehicle and the surrounding objects is determined according to the second return signal.
According to the embodiment, the MEMS laser radar and the radar of the vehicle are compensated mutually, and the accuracy of the measurement result of the laser radar is improved.
In an embodiment, determining, based on a preset kalman filter, an actually measured position of the vehicle at the current positioning time according to the first distance information, the second distance information, and the previous positioning information includes:
constructing an observation vector of a Kalman filter according to the positioning information;
constructing a state vector of the Kalman filter based on the first distance information and the second distance information;
calculating a Kalman gain matrix of a Kalman filter based on the state vector;
and filtering the observation vector based on a Kalman gain matrix of a Kalman filter to obtain the actually measured position.
In this embodiment, an observation vector is constructed based on the previous positioning information, and the previous positioning information is corrected by using a state vector constructed by the first distance information and the second distance information, so that the actual measurement position of the vehicle at the current moment is obtained, noise interference is reduced, and the positioning accuracy is improved.
In one embodiment, a kalman gain matrix of a kalman filter is calculated based on the state vector, comprising:
calculating a state transition matrix of the Kalman filter according to the state vector;
updating a covariance matrix of the state vector according to the state transition matrix;
and calculating a Kalman gain matrix of the Kalman filter according to the covariance matrix.
In the embodiment, the Kalman gain matrix of the Kalman filter is finally calculated through the state vector, so that the noise influence is eliminated, and the data accuracy is improved.
In one embodiment, predicting a predicted position of a vehicle at a current location time based on an inertial navigation system of the vehicle includes:
acquiring motion state information and last positioning information of a vehicle;
carrying out strapdown resolving on the motion state information to obtain the attitude information of the vehicle;
and predicting the predicted position of the vehicle at the current positioning moment based on the last positioning information and the attitude information of the vehicle.
In the embodiment, vehicle attitude information is obtained by performing strapdown calculation on the motion state information, and the prediction of the predicted position of the vehicle is realized by combining the last positioning information.
In one embodiment, the motion state information includes position coordinates, three-dimensional motion speed, and a posture quaternion, and the strapdown calculation is performed on the motion state information to obtain the posture information of the vehicle, including:
performing derivation on the position coordinate, the three-dimensional motion speed and the attitude quaternion to obtain a first derivation result, wherein the derivation result of the three-dimensional motion speed is a three-dimensional motion acceleration;
performing deviation correction on the three-dimensional motion acceleration in the first derivation result to obtain a second derivation result;
and performing fourth-order approximate operation on the second derivation result to obtain the attitude information of the vehicle.
In one embodiment, the fusing the measured position and the predicted position to obtain the current positioning information of the vehicle includes:
determining a first weight of the measured position and a second weight of the predicted position based on a preset filter;
and calculating the current positioning information of the vehicle according to the measured position, the first weight, the predicted position and the second weight.
In the embodiment, the filter is preset to determine the weight, and the current positioning information is calculated based on the weight value, so that the noise interference is effectively reduced and the positioning accuracy is improved.
In an embodiment, before acquiring the first return signal received by the laser radar, the method further includes:
controlling a laser radar to emit a laser signal; the laser signal is reflected and received by the lidar as a first return signal.
In one embodiment, the laser radar adopts an MEMS laser radar, the MEMS laser radar comprises a micro vibrating mirror, and the micro vibrating mirror is driven by electromagnetism;
controlling the laser radar to emit the laser signal comprises: and driving a micro-vibration mirror of the MEMS laser radar to reflect the laser signal to a target direction.
In this embodiment, the reception of the first return signal is achieved by controlling the MEMS lidar.
In a second aspect, an embodiment of the present application provides a vehicle positioning apparatus, including:
the acquisition module is used for acquiring a first return signal received by the laser radar;
the determining module is used for determining the actual measurement position of the vehicle at the current positioning moment according to the first return signal;
the prediction module is used for predicting the predicted position of the vehicle at the current positioning moment according to an inertial navigation system of the vehicle;
and the fusion module is used for fusing the actual measurement position and the prediction position to obtain the current positioning information of the vehicle.
In one embodiment, the determining module includes:
the first determining submodule is used for determining first distance information between the vehicle and surrounding objects according to the first return signal;
the first obtaining submodule is used for obtaining second distance information between the vehicle and surrounding objects at the last positioning moment and last positioning information of the vehicle at the last positioning moment;
and the second determining submodule is used for determining the actual measurement position of the vehicle at the current positioning moment according to the first distance information, the second distance information and the last positioning information based on a preset Kalman filter.
In an embodiment, the obtaining module is further configured to: acquiring a second return signal received by the vehicle-mounted radar;
and the determining module is also used for determining the actual measurement position of the vehicle at the current positioning moment according to the second return signal.
In one embodiment, the first determining sub-module includes:
and the determining unit is used for determining first distance information between the vehicle and the surrounding objects according to the second return signals.
In one embodiment, the second determining sub-module includes:
the first construction unit is used for constructing an observation vector of the Kalman filter according to the positioning information;
the second construction unit is used for constructing a state vector of the Kalman filter based on the first distance information and the second distance information;
the computing unit is used for computing a Kalman gain matrix of the Kalman filter based on the state vector;
and the filtering unit is used for filtering the observation vector based on a Kalman gain matrix of the Kalman filter to obtain the actual measurement position.
In one embodiment, a computing unit includes:
the first calculating subunit is used for calculating a state transition matrix of the Kalman filter according to the state vector;
the updating subunit is used for updating the covariance matrix of the state vector according to the state transition matrix;
and the second calculation subunit is used for calculating a Kalman gain matrix of the Kalman filter according to the covariance matrix.
In one embodiment, the prediction module comprises:
the second acquisition submodule is used for acquiring the motion state information and the last positioning information of the vehicle;
the resolving submodule is used for carrying out strapdown resolving on the motion state information to obtain the attitude information of the vehicle;
and the prediction submodule is used for predicting the predicted position of the vehicle at the current positioning moment based on the previous positioning information and the attitude information of the vehicle.
In one embodiment, the motion state information includes position coordinates, three-dimensional motion velocity, and attitude quaternion, and the resolving submodule includes:
the derivation unit is used for performing derivation on the position coordinate, the three-dimensional motion speed and the attitude quaternion to obtain a first derivation result, wherein the derivation result of the three-dimensional motion speed is a three-dimensional motion acceleration;
the correction unit is used for carrying out deviation correction on the three-dimensional motion acceleration in the first derivation result to obtain a second derivation result;
and the operation unit is used for performing fourth-order approximate operation on the second derivation result to obtain the attitude information of the vehicle.
In one embodiment, a fusion module includes:
the third determining submodule is used for determining the first weight of the measured position and the second weight of the predicted position based on the preset filter;
and the calculation submodule is used for calculating the current positioning information of the vehicle according to the measured position, the first weight, the predicted position and the second weight.
In one embodiment, the vehicle positioning apparatus further comprises:
the control module is used for controlling the laser radar to transmit laser signals; the laser signal is reflected and received by the lidar as a first return signal.
In one embodiment, the laser radar adopts an MEMS laser radar, the MEMS laser radar comprises a micro vibrating mirror, and the micro vibrating mirror is driven by electromagnetism;
and the driving module is used for driving a micro-vibration mirror of the MEMS laser radar to reflect the laser signal in a target direction.
In a third aspect, an embodiment of the present application provides a vehicle, where the vehicle is provided with an MEMS lidar, a vehicle-mounted radar, and an inertial navigation system; the MEMS laser radar is arranged to emit a laser signal and receive a first return signal; the vehicle-mounted radar is used for transmitting a second signal and receiving a second return signal; the inertial navigation system is used for predicting the predicted position of the vehicle at the current positioning moment; a processor is also included, the processor being connected to the solid state lidar, the vehicle radar and the inertial navigation system for performing the method of the first aspect.
In a fourth aspect, an embodiment of the present application provides an electronic device, including a memory and a processor, where the memory is used to store a computer program, and the processor runs the computer program to make the electronic device execute the vehicle positioning method of the first aspect.
In a fifth aspect, an embodiment of the present application provides a computer-readable storage medium, which stores a computer program, and the computer program, when executed by a processor, implements the vehicle positioning method of the first aspect.
Please refer to the description of the first aspect for the advantageous effects of the second to fifth aspects.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments of the present application will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and that those skilled in the art can also obtain other related drawings based on the drawings without inventive efforts.
FIG. 1 is a schematic flow chart illustrating a vehicle positioning method according to an embodiment of the present disclosure;
FIG. 2 is a schematic structural diagram of a vehicle positioning device provided in an embodiment of the present application;
fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.
It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined and explained in subsequent figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only for distinguishing the description, and are not to be construed as indicating or implying relative importance.
As described in the related art, the application of MEMS solid state lidar to autonomous vehicles has higher resolution, safety, and lower cost. However, the MEMS lidar And the inertial system usually operate in a synchronous positioning And Mapping (SLAM) manner, And the MEMS solid-state lidar tends to have a null shift And poor measurement accuracy, resulting in poor positioning accuracy.
In order to solve the problems in the prior art, the application provides a vehicle positioning method, which includes the steps that a first return signal received by a Micro Electro Mechanical System (MEMS) laser radar is obtained, and the actually measured position of a vehicle at the current positioning moment is determined according to the first return signal, so that the MEMS laser radar and a vehicle-mounted radar are mutually compensated, and the accuracy of the measurement result of the laser radar is improved; and then, according to an inertial navigation system of the vehicle, predicting the predicted position of the vehicle at the current positioning moment, and fusing the actual measurement position and the predicted position to obtain the current positioning information of the vehicle, so that the actual measurement position and the predicted position are utilized to correct the error of the final positioning result, and the positioning precision of the vehicle is improved.
Referring to fig. 1, fig. 1 shows a flowchart for implementing a vehicle positioning method provided by an embodiment of the present application. The vehicle positioning method described in the embodiments of the present application can be applied to electronic devices including, but not limited to, computer devices such as smart phones, tablet computers, desktop computers, supercomputers, personal digital assistants, physical servers, and cloud servers, which are communicatively connected to a vehicle. The vehicle positioning method of the embodiment of the application comprises the following steps of S101 to S104:
and step S101, acquiring a first return signal received by the laser radar.
In the step, the laser radar is installed on the vehicle, and the laser radar is used for sending the laser signal and receiving the first return signal returned after the laser signal is reflected by the object.
Optionally, the laser radar is a MEMS laser radar, and the MEMS laser radar includes a micro-oscillating mirror, and the micro-oscillating mirror is electromagnetically driven.
In one embodiment, the MEMS laser radar is controlled to emit a laser signal, and the laser signal can be reflected by an object to be a first return signal; driving a micro-vibration mirror of the MEMS laser radar to reflect the laser signal to a target direction; a first return signal is received by the MEMS lidar that the laser signal is reflected.
In this embodiment, compare in mechanical type laser radar's polygon prism and pendulum mirror, that is to say, how many pencil are realized to traditional mechanical type laser radar, just need how many groups emission module and receiving module, MEMS laser radar then only needs a beam laser source, reflects the light beam of laser instrument through the little mirror that shakes, makes both adopt the same frequency collaborative work, reaches the purpose of carrying out 3D scanning to the target object after receiving through the detector. Compared with the structure of the mechanical laser radar with multiple groups of transmitting modules and receiving modules, the MEMS laser radar has the advantages that the requirements for the number of lasers and detectors are obviously reduced, and the hardware cost is reduced.
After the size of the micro-vibration mirror of the MEMS laser radar is reduced, the optical caliber and the scanning angle of the MEMS laser radar can be limited, so that the view field angle is reduced. Therefore, according to the embodiment, the vibrating mirror with the larger size is driven through electromagnetism, the speed and the position are matched according to the characteristics of the fast axis and the slow axis, the laser beam is regularly projected to the designated direction, the first return signal is recovered through the receiver, and the first return signal is obtained based on the MEMS laser radar.
And step S102, determining the actual measurement position of the vehicle at the current positioning moment according to the first return signal.
In this embodiment, since the first return signal is a signal received by the receiver after the laser signal is reflected by the reflective object, the measured position of the vehicle relative to the reflective object can be determined based on the first return signal.
Step S103, predicting the predicted position of the vehicle at the current positioning time according to the inertial navigation system of the vehicle.
In this embodiment, the inertial navigation system is an inertial navigation system including inertial devices such as an acceleration sensor and an angular velocity sensor on a vehicle. The inertial navigation system based on the vehicle can obtain the motion state data of the vehicle, and then utilizes a mathematical derivation mode to conduct derivation on the motion state data so as to predict the position of the vehicle.
And step S104, fusing the actual measurement position and the prediction position to obtain the current positioning information of the vehicle.
In this embodiment, since both the actual measurement position and the predicted position may have measurement errors, in order to reduce the errors, the actual measurement position and the predicted position are fused to compensate each other, so as to achieve the purpose of improving the positioning accuracy.
Optionally, the current positioning information of the vehicle is obtained by performing weighted averaging on the measured position and the predicted position. Further, a filter mode may be adopted to determine the weight values of the actual measurement position and the predicted position, and the actual measurement position and the predicted position are weighted and averaged based on the weight values.
In an embodiment, on the basis of the embodiment in fig. 1, the step S102 includes: determining first distance information of the vehicle and surrounding objects according to the first return signal; acquiring second distance information between the vehicle and surrounding objects at the last positioning moment and last positioning information of the vehicle at the last positioning moment; and determining the actual measurement position of the vehicle at the current positioning moment according to the first distance information, the second distance information and the last positioning information based on a preset Kalman filter.
In this embodiment, the kalman filter is an optimal linear state estimation method, that is, a state vector best fitting to observed data is solved in a mathematical manner, which can reduce noise interference, thereby improving the accuracy of data processing.
Optionally, a first distance value between the vehicle and the surrounding object is determined through the first return signal, a second distance value between the vehicle and the surrounding object is determined through the second return signal, and then the first distance information is obtained by solving an optimal value between the first distance value and the second distance value through a kalman filter. And correcting the last positioning information through the first distance information and the second distance information to determine the actual measurement position of the vehicle at the current positioning moment.
Determining first distance information of the vehicle from surrounding objects, comprising: acquiring a second return signal received by the vehicle-mounted radar; and determining first distance information of the vehicle and surrounding objects according to the first return signal and the second return signal.
The vehicle-mounted radar is a radar carried by the vehicle when leaving a factory. And determining a first position of the vehicle relative to the reflecting object based on the first return signal, determining a second position of the vehicle relative to the reflecting object based on the second return signal, and performing mutual compensation (for example, by adopting a weighting and averaging manner) on the first position and the second position to finally obtain the measured position of the vehicle.
Optionally, the observation vectors of the first position and the second position are constructed based on a kalman filter to solve the optimal value of the measured position, so as to improve the positioning accuracy of the vehicle.
It can be understood that the radar carried by the vehicle body is generally used for short-distance anti-collision detection, and the collected data is directly used for judgment without modeling; according to the method, the MEMS laser radar is installed on the vehicle to be used for modeling collected data, so that the purpose of improving the positioning accuracy by mutually compensating with the radar of the vehicle body is achieved.
Optionally, determining, based on a preset kalman filter, an actually measured position of the vehicle at the current positioning time according to the first distance information, the second distance information, and the previous positioning information, where the determining includes: constructing an observation vector of a Kalman filter according to the positioning information; constructing a state vector of the Kalman filter based on the first distance information and the second distance information; calculating a Kalman gain matrix of a Kalman filter based on the state vector; and filtering the observation vector based on a Kalman gain matrix of a Kalman filter to obtain the actually measured position.
In the present embodiment, illustratively, the observation vector Z is established based on the above positioning informationtEstablishing a state vector X according to the first distance information and the second distance informationt. Since the sensor has an interference signal, which can be regarded as a fixed value, the adverse effect of this interference on the measured position can be corrected. And inputting the observation vector and the state vector into a Kalman filter to update the state of the last positioning information, and finally obtaining the actual measurement position. The present embodiment corrects the error influence by using the first distance information and the second distance information as the state vector, thereby improving the positioning measurement accuracy.
Optionally, calculating a kalman gain matrix of the kalman filter based on the state vector, including: calculating a state transition matrix of the Kalman filter according to the state vector; updating a covariance matrix of the state vector according to the state transition matrix; and calculating a Kalman gain matrix of the Kalman filter according to the covariance matrix.
In the present embodiment, for example, according to the time recursion, it can be found that: x is the number oft＝xt-1+dxt-1-lt，dxt＝dxt-1，xtIndicating the current time, dxt-1Representing time increments, xt-1Indicates the last time, thereforeThe kinematic formula of the state transition matrix may determine the state transition matrix as:
updating covariance matrix of state vector based on state transition matrix
Where Q is the system noise. Calculating a Kalman gain matrix:
in an embodiment, on the basis of the embodiment in fig. 1, the step S103 includes: acquiring motion state information and last positioning information of a vehicle; carrying out strapdown resolving on the motion state information to obtain the attitude information of the vehicle; and predicting the predicted position of the vehicle at the current positioning moment based on the last positioning information and the attitude information of the vehicle.
In the present embodiment, the strapdown solution is a strapdown inertial navigation solution, which indicates a fixed connection with a carrier (vehicle). The motion state information of the embodiment comprises position coordinates, three-dimensional motion speed and a posture quaternion, and the strapdown calculation process can be used for obtaining the posture information of the vehicle by deriving the motion state information, so that the positioning independent of environment information is realized.
Optionally, performing derivation on the position coordinate, the three-dimensional motion speed and the attitude quaternion to obtain a first derivation result, wherein the derivation result of the three-dimensional motion speed is a three-dimensional motion acceleration; performing deviation correction on the three-dimensional motion acceleration in the first derivation result to obtain a second derivation result; and performing fourth-order approximate operation on the second derivation result to obtain the attitude information of the vehicle. And predicting the predicted position of the vehicle at the current positioning moment based on the last positioning information and the attitude information of the vehicle.
In an embodiment, on the basis of the embodiment in fig. 1, the step S104 includes: determining a first weight of the measured position and a second weight of the predicted position based on a preset filter; and calculating the current positioning information of the vehicle according to the measured position, the first weight, the predicted position and the second weight.
In this embodiment, a preset filter compares the predicted position with the measured position measured by the sensor. The predicted and measured positions are combined to give an updated position. Alternatively, depending on the uncertainty of each value, the preset filter may be a kalman filter, which will apply more weight to either the predicted or measured position, then another sensor measurement will be received after a period of time Δ t, and the algorithm then proceeds with another prediction and update step.
In order to execute the method corresponding to the above method embodiment to achieve the corresponding function and technical effect, a vehicle positioning device is provided below. Referring to fig. 2, fig. 2 is a block diagram of a vehicle positioning device according to an embodiment of the present disclosure. The modules included in the apparatus in this embodiment are used to execute the steps in the embodiment corresponding to fig. 1, and refer to fig. 1 and the related description in the embodiment corresponding to fig. 1 specifically. For convenience of explanation, only the parts related to the present embodiment are shown, and the vehicle positioning apparatus provided in the embodiment of the present application includes:
an obtaining module 201, configured to obtain a first return signal received by a laser radar;
the determining module 202 is configured to determine, according to the first return signal, an actually measured position of the vehicle at the current positioning time;
the prediction module 203 is used for predicting the predicted position of the vehicle at the current positioning moment according to the inertial navigation system of the vehicle;
and the fusion module 204 is configured to fuse the measured position and the predicted position to obtain current positioning information of the vehicle.
In one embodiment, the determining module 202 includes:
the first determining submodule is used for determining first distance information between the vehicle and surrounding objects according to the first return signal;
the first obtaining submodule is used for obtaining second distance information between the vehicle and surrounding objects at the last positioning moment and last positioning information of the vehicle at the last positioning moment;
and the second determining submodule is used for determining the actual measurement position of the vehicle at the current positioning moment according to the first distance information, the second distance information and the last positioning information based on a preset Kalman filter.
In an embodiment, the obtaining module 201 is further configured to: acquiring a second return signal received by the vehicle-mounted radar;
the determining module 202 is further configured to determine, according to the second return signal, a measured position of the vehicle at the current positioning time.
In one embodiment, the first determining sub-module includes:
and the determining unit is used for determining first distance information between the vehicle and the surrounding objects according to the second return signals.
In one embodiment, the second determining sub-module includes:
the first construction unit is used for constructing an observation vector of the Kalman filter according to the positioning information;
the second construction unit is used for constructing a state vector of the Kalman filter based on the first distance information and the second distance information;
the computing unit is used for computing a Kalman gain matrix of the Kalman filter based on the state vector;
and the filtering unit is used for filtering the observation vector based on a Kalman gain matrix of the Kalman filter to obtain the actual measurement position.
In one embodiment, a computing unit includes:
the first calculating subunit is used for calculating a state transition matrix of the Kalman filter according to the state vector;
the updating subunit is used for updating the covariance matrix of the state vector according to the state transition matrix;
and the second calculation subunit is used for calculating a Kalman gain matrix of the Kalman filter according to the covariance matrix.
In one embodiment, the prediction module 203 comprises:
the second acquisition submodule is used for acquiring the motion state information and the last positioning information of the vehicle;
the resolving submodule is used for carrying out strapdown resolving on the motion state information to obtain the attitude information of the vehicle;
and the prediction submodule is used for predicting the predicted position of the vehicle at the current positioning moment based on the previous positioning information and the attitude information of the vehicle.
In one embodiment, the motion state information includes position coordinates, three-dimensional motion velocity, and attitude quaternion, and the resolving submodule includes:
the derivation unit is used for performing derivation on the position coordinate, the three-dimensional motion speed and the attitude quaternion to obtain a first derivation result, wherein the derivation result of the three-dimensional motion speed is a three-dimensional motion acceleration;
the correction unit is used for carrying out deviation correction on the three-dimensional motion acceleration in the first derivation result to obtain a second derivation result;
and the operation unit is used for performing fourth-order approximate operation on the second derivation result to obtain the attitude information of the vehicle.
In one embodiment, the fusion module 204 includes:
the third determining submodule is used for determining the first weight of the measured position and the second weight of the predicted position based on the preset filter;
and the calculation submodule is used for calculating the current positioning information of the vehicle according to the measured position, the first weight, the predicted position and the second weight.
In one embodiment, the vehicle positioning apparatus further comprises:
the control module is used for controlling the laser radar to transmit laser signals; the laser signal is reflected and received by the lidar as a first return signal.
In one embodiment, the laser radar adopts an MEMS laser radar, the MEMS laser radar comprises a micro vibrating mirror, and the micro vibrating mirror is driven by electromagnetism;
and the driving module is used for driving a micro-vibration mirror of the MEMS laser radar to reflect the laser signal in a target direction.
The vehicle positioning device can implement the vehicle positioning method of the embodiment of the method. The alternatives in the above-described method embodiments are also applicable to this embodiment and will not be described in detail here. The rest of the embodiments of the present application may refer to the contents of the above method embodiments, and in this embodiment, details are not described again.
Fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the present application. As shown in fig. 3, the electronic apparatus 3 of this embodiment includes: at least one processor 30 (only one shown in fig. 3), a memory 31, and a computer program 32 stored in the memory 31 and executable on the at least one processor 30, the processor 30 implementing the steps of any of the above-described method embodiments when executing the computer program 32.
The electronic device 3 may be a computing device such as a smart phone, a tablet computer, a desktop computer, a supercomputer, a personal digital assistant, a physical server, and a cloud server. The electronic device may include, but is not limited to, a processor 30, a memory 31. Those skilled in the art will appreciate that fig. 3 is only an example of the electronic device 3, and does not constitute a limitation to the electronic device 3, and may include more or less components than those shown, or combine some components, or different components, such as an input-output device, a network access device, and the like.
The Processor 30 may be a Central Processing Unit (CPU), and the Processor 30 may be other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, discrete hardware component, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The memory 31 may in some embodiments be an internal storage unit of the electronic device 3, such as a hard disk or a memory of the electronic device 3. The memory 31 may also be an external storage device of the electronic device 3 in other embodiments, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), and the like, which are provided on the electronic device 3. Further, the memory 31 may also include both an internal storage unit and an external storage device of the electronic device 3. The memory 31 is used for storing an operating system, an application program, a BootLoader (BootLoader), data, and other programs, such as program codes of the computer program. The memory 31 may also be used to temporarily store data that has been output or is to be output.
In addition, an embodiment of the present application further provides a computer-readable storage medium, where a computer program is stored, and when the computer program is executed by a processor, the computer program implements the steps in any of the method embodiments described above.
The embodiments of the present application provide a computer program product, which when running on an electronic device, enables the electronic device to implement the steps in the above method embodiments when executed.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative, and for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, functional modules in the embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application or portions thereof that substantially contribute to the prior art may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present application shall be included in the protection scope of the present application. It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, it need not be further defined and explained in subsequent figures.
The above description is only for the specific embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present application, and shall be covered by the scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.
It is noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.
Claims (25)
1. A vehicle positioning method, characterized by comprising:
acquiring a first return signal received by a laser radar;
determining the actual measurement position of the vehicle at the current positioning moment according to the first return signal;
predicting a predicted position of the vehicle at a current positioning moment according to an inertial navigation system of the vehicle;
and fusing the measured position and the predicted position to obtain the current positioning information of the vehicle.
2. The vehicle positioning method of claim 1, wherein determining the measured position of the vehicle at the current positioning time comprises:
determining first distance information of the vehicle and surrounding objects according to the first return signal;
acquiring second distance information between the vehicle and the surrounding objects at the last positioning moment and last positioning information of the vehicle at the last positioning moment;
and determining the actual measurement position of the vehicle at the current positioning moment according to the first distance information, the second distance information and the last positioning information based on a preset Kalman filter.
3. The vehicle positioning method according to claim 2, characterized by further comprising:
acquiring a second return signal received by the vehicle-mounted radar;
the determining the measured position of the vehicle at the current positioning moment includes: and determining the actual measurement position of the vehicle at the current positioning moment according to the second return signal.
4. The vehicle localization method of claim 3, wherein the determining the first distance information of the vehicle from surrounding objects comprises:
and determining first distance information of the vehicle and surrounding objects according to the second return signal.
5. The vehicle positioning method according to any one of claims 2 to 4, wherein the determining, based on a preset Kalman filter, a measured position of the vehicle at a current positioning time according to the first distance information, the second distance information and the last positioning information includes:
constructing an observation vector of the Kalman filter according to the upper positioning information;
constructing a state vector of the Kalman filter based on the first distance information and the second distance information;
calculating a Kalman gain matrix of the Kalman filter based on the state vector;
and filtering the observation vector based on a Kalman gain matrix of the Kalman filter to obtain the actually measured position.
6. The vehicle positioning method of claim 5, wherein the computing a Kalman gain matrix of the Kalman filter based on the state vector comprises:
calculating a state transition matrix of the Kalman filter according to the state vector;
updating a covariance matrix of the state vector according to the state transition matrix;
and calculating a Kalman gain matrix of the Kalman filter according to the covariance matrix.
7. The vehicle positioning method according to any one of claims 1 to 4, wherein the predicting the predicted position of the vehicle at the current positioning time according to the inertial navigation system of the vehicle includes:
acquiring the motion state information and the last positioning information of the vehicle;
performing strapdown resolving on the motion state information to obtain attitude information of the vehicle;
and predicting the predicted position of the vehicle at the current positioning moment based on the last positioning information and the attitude information of the vehicle.
8. The vehicle positioning method according to claim 7, wherein the motion state information includes position coordinates, three-dimensional motion speed, and attitude quaternion, and the performing strapdown solution on the motion state information to obtain the attitude information of the vehicle includes:
performing derivation on the position coordinate, the three-dimensional motion speed and the attitude quaternion to obtain a first derivation result, wherein the derivation result of the three-dimensional motion speed is a three-dimensional motion acceleration;
performing deviation correction on the three-dimensional motion acceleration in the first derivation result to obtain a second derivation result;
and performing fourth-order approximate operation on the second derivation result to obtain the attitude information of the vehicle.
9. The vehicle positioning method according to any one of claims 1 to 4, wherein the fusing the measured position and the predicted position to obtain the current positioning information of the vehicle comprises:
determining a first weight of the measured position and a second weight of the predicted position based on a preset filter;
and calculating the current positioning information of the vehicle according to the measured position, the first weight, the predicted position and the second weight.
10. The vehicle positioning method according to claim 1, wherein before the acquiring the first return signal received by the lidar, further comprises:
controlling the laser radar to emit a laser signal; and the laser signal is reflected and then received by the laser radar as the first return signal.
11. The vehicle positioning method according to claim 10, wherein the lidar employs a MEMS lidar including a micro-galvanometer driven electromagnetically;
the controlling the laser radar to emit the laser signal includes: and driving a micro-vibration mirror of the MEMS laser radar to reflect the laser signal to a target direction.
12. A vehicle positioning device, comprising:
the acquisition module is used for acquiring a first return signal received by the laser radar;
the determining module is used for determining the actual measurement position of the vehicle at the current positioning moment according to the first return signal;
the prediction module is used for predicting the predicted position of the vehicle at the current positioning moment according to the inertial navigation system of the vehicle;
and the fusion module is used for fusing the measured position and the predicted position to obtain the current positioning information of the vehicle.
13. The vehicle locating apparatus of claim 12, wherein the determining module comprises:
the first determining submodule is used for determining first distance information between the vehicle and surrounding objects according to the first return signal;
the first obtaining submodule is used for obtaining second distance information between the vehicle and the surrounding objects at the last positioning time and last positioning information of the vehicle at the last positioning time;
and the second determining submodule is used for determining the actual measurement position of the vehicle at the current positioning moment according to the first distance information, the second distance information and the last positioning information based on a preset Kalman filter.
14. The vehicle locating apparatus of claim 12, wherein the obtaining module is further configured to: acquiring a second return signal received by the vehicle-mounted radar;
and the determining module is also used for determining the actual measurement position of the vehicle at the current positioning moment according to the second return signal.
15. The vehicle locating apparatus of claim 14, wherein the first determining submodule includes:
and the determining unit is used for determining first distance information between the vehicle and surrounding objects according to the second return signal.
16. The vehicle localization apparatus according to any one of claims 13 to 15, wherein the second determination submodule includes:
the first construction unit is used for constructing an observation vector of the Kalman filter according to the upper positioning information;
a second constructing unit, configured to construct a state vector of the kalman filter based on the first distance information and the second distance information;
a calculation unit for calculating a Kalman gain matrix of the Kalman filter based on the state vector;
and the filtering unit is used for filtering the observation vector based on a Kalman gain matrix of the Kalman filter to obtain the actual measurement position.
17. The vehicle localization apparatus of claim 16, wherein the computing unit comprises:
the first calculating subunit is used for calculating a state transition matrix of the Kalman filter according to the state vector;
the updating subunit is used for updating the covariance matrix of the state vector according to the state transition matrix;
and the second calculation subunit is used for calculating a Kalman gain matrix of the Kalman filter according to the covariance matrix.
18. The vehicle localization apparatus of any of claims 12 to 15, wherein the prediction module comprises:
the second acquisition submodule is used for acquiring the motion state information and the last positioning information of the vehicle;
the resolving submodule is used for carrying out strapdown resolving on the motion state information to obtain the attitude information of the vehicle;
and the predicting submodule is used for predicting the predicted position of the vehicle at the current positioning moment based on the previous positioning information and the attitude information of the vehicle.
19. The vehicle localization apparatus according to claim 18, wherein the motion state information includes position coordinates, three-dimensional motion velocity, and attitude quaternion, and the resolving submodule includes:
the derivation unit is used for performing derivation on the position coordinate, the three-dimensional motion speed and the attitude quaternion to obtain a first derivation result, wherein the derivation result of the three-dimensional motion speed is a three-dimensional motion acceleration;
the correction unit is used for carrying out deviation correction on the three-dimensional motion acceleration in the first derivation result to obtain a second derivation result;
and the operation unit is used for performing fourth-order approximate operation on the second derivation result to obtain the attitude information of the vehicle.
20. The vehicle localization apparatus of any of claims 12 to 15, wherein the fusion module comprises:
a third determining submodule, configured to determine, based on a preset filter, a first weight of the measured position and a second weight of the predicted position;
and the calculation submodule is used for calculating the current positioning information of the vehicle according to the measured position, the first weight, the predicted position and the second weight.
21. The vehicle locating apparatus of claim 12, further comprising:
the control module is used for controlling the laser radar to transmit laser signals; and the laser signal is reflected and then received by the laser radar as the first return signal.
22. The vehicle positioning apparatus of claim 19, wherein the lidar employs a MEMS lidar including a micro-galvanometer driven electromagnetically;
and the driving module is used for driving a micro-vibration mirror of the MEMS laser radar to reflect the laser signal in a target direction.
23. A vehicle, characterized in that the vehicle is provided with a MEMS lidar, a vehicle radar and an inertial navigation system; the MEMS lidar is configured to transmit a laser signal and receive a first return signal; the vehicle-mounted radar is used for transmitting a second signal and receiving a second return signal; the inertial navigation system is used for predicting the predicted position of the vehicle at the current positioning moment; the system further comprises a processor connected with the solid state laser radar, the vehicle-mounted radar and the inertial navigation system and used for executing the method of any one of claims 1-11.
24. An electronic device, comprising a memory for storing a computer program and a processor for executing the computer program to cause the electronic device to perform the vehicle positioning method according to any one of claims 1 to 11.
25. A computer-readable storage medium, characterized in that it stores a computer program which, when being executed by a processor, implements the vehicle positioning method according to any one of claims 1 to 11.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111509693.3A CN114063110A (en) | 2021-12-10 | 2021-12-10 | Vehicle positioning method and device, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111509693.3A CN114063110A (en) | 2021-12-10 | 2021-12-10 | Vehicle positioning method and device, electronic equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114063110A true CN114063110A (en) | 2022-02-18 |
Family
ID=80229109
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111509693.3A Pending CN114063110A (en) | 2021-12-10 | 2021-12-10 | Vehicle positioning method and device, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114063110A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114945154A (en) * | 2022-05-31 | 2022-08-26 | 中国移动通信集团江苏有限公司 | Vehicle position prediction method, device, electronic equipment and computer program product |
-
2021
- 2021-12-10 CN CN202111509693.3A patent/CN114063110A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114945154A (en) * | 2022-05-31 | 2022-08-26 | 中国移动通信集团江苏有限公司 | Vehicle position prediction method, device, electronic equipment and computer program product |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109975792B (en) | Method for correcting point cloud motion distortion of multi-line laser radar based on multi-sensor fusion | |
| CN111721289B (en) | Vehicle positioning method, device, equipment, storage medium and vehicle in automatic driving | |
| US10788830B2 (en) | Systems and methods for determining a vehicle position | |
| CN111413721B (en) | Vehicle positioning method, device, controller, intelligent vehicle and system | |
| CN110207714A (en) | A kind of method, onboard system and the vehicle of determining vehicle pose | |
| CN112781586B (en) | Pose data determination method and device, electronic equipment and vehicle | |
| KR101909953B1 (en) | Method for vehicle pose estimation using LiDAR | |
| US9513130B1 (en) | Variable environment high integrity registration transformation system and related method | |
| CN111308415B (en) | Online pose estimation method and equipment based on time delay | |
| CN109141411B (en) | Positioning method, positioning device, mobile robot, and storage medium | |
| CN112946681B (en) | Laser radar positioning method fusing combined navigation information | |
| KR101962398B1 (en) | Method and Apparatus for Compensating Target Information Error of Radar | |
| CN113052897A (en) | Positioning initialization method and related device, equipment and storage medium | |
| CN110895408A (en) | Autonomous positioning method and device and mobile robot | |
| CN115950418A (en) | Multi-sensor fusion positioning method | |
| CN114063110A (en) | Vehicle positioning method and device, electronic equipment and storage medium | |
| CN114942025A (en) | Vehicle navigation positioning method and device, electronic equipment and storage medium | |
| CN113218389B (en) | Vehicle positioning method, device, storage medium and computer program product | |
| CN113008244B (en) | Navigation information processing method and device, electronic equipment and storage medium | |
| Lee et al. | LiDAR odometry survey: recent advancements and remaining challenges | |
| CN116184430B (en) | Pose estimation algorithm fused by laser radar, visible light camera and inertial measurement unit | |
| CN117387604A (en) | Positioning and mapping method and system based on 4D millimeter wave radar and IMU fusion | |
| CN116047481A (en) | Method, device, equipment and storage medium for correcting point cloud data distortion | |
| CN113227711A (en) | Navigation device, navigation parameter calculation method, and program | |
| CN114088104B (en) | Map generation method under automatic driving scene |
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 |