Vehicle positioning method, vehicle positioning device and terminal equipmentTechnical Field
The present application belongs to the field of vehicle navigation technologies, and in particular, to a vehicle positioning method, a vehicle positioning apparatus, a terminal device, and a computer-readable storage medium.
Background
In the process of carrying out route navigation on a vehicle by a navigation system of the vehicle, a Global Positioning System (GPS) for positioning the vehicle needs to calculate the position and the speed of the vehicle according to received navigation satellite signals, and the navigation satellite signals are easy to lose lock in special scenes such as tunnels, underground garages, annular viaducts and the like and are easy to be interfered by external signals, so that the navigation system of the vehicle is difficult to provide accurate positioning of the vehicle; however, the navigation error of the inertial navigation INS continues to accumulate with time, and it is difficult to provide long-term positioning accuracy.
Disclosure of Invention
In view of the above, the present application provides a vehicle positioning method, a vehicle positioning apparatus, a terminal device and a computer-readable storage medium to improve the accuracy of vehicle positioning.
A first aspect of the present application provides a vehicle positioning method, including:
acquiring a vehicle state after primary correction, wherein the vehicle state comprises speed, position, direction and posture;
detecting the motion state of the vehicle in real time;
when the vehicle is detected to be in a running state, performing secondary correction on the vehicle state after the primary correction based on virtual constraint correction;
when the vehicle is detected to be in a static state, performing secondary correction on the vehicle state after the primary correction based on zero-speed correction;
and positioning the vehicle based on the vehicle state after the secondary correction.
Optionally, the obtaining the vehicle state after the primary correction includes:
in a static state of a vehicle, carrying out online calibration and error compensation on an inertial sensor of the vehicle so as to realize real-time correction of output data of a gyroscope and an accelerometer in the inertial sensor and finish initialization of zero offset data of the gyroscope;
in the straight-line running state of the vehicle, the inertial sensors are initially aligned to realize the initialization of each parameter in the integrated navigation module;
and updating the vehicle state according to the output data of the gyroscope and the accelerometer which are corrected in real time to obtain the vehicle state after primary correction.
Optionally, the detecting the motion state of the vehicle in real time includes:
acquiring a standard deviation of output data of the gyroscope corrected in real time within a preset time, and recording the standard deviation as an angular velocity standard deviation;
acquiring a standard deviation of output data of the accelerometer corrected in real time within a preset time, and recording the standard deviation as an acceleration standard deviation;
obtaining the engine speed and the vehicle speed
And detecting the motion state of the vehicle in real time according to the angular speed standard deviation, the acceleration standard deviation, the engine rotating speed and/or the vehicle speed.
Optionally, the detecting a motion state of the vehicle in real time according to the angular velocity standard deviation, the acceleration standard deviation, the engine speed, and the vehicle speed includes:
and if the angular speed standard deviation is greater than a preset first angular speed standard deviation threshold value, the acceleration standard deviation is greater than a preset first acceleration standard deviation threshold value, and the vehicle speed is greater than 0, determining that the vehicle is in a running state.
Optionally, the detecting a motion state of the vehicle in real time according to the angular velocity standard deviation, the acceleration standard deviation, the engine speed, and the vehicle speed includes:
if the angular velocity standard deviation is not larger than a preset second angular velocity standard deviation threshold value, and the acceleration standard deviation is not larger than a preset second acceleration standard deviation threshold value, determining that the vehicle is in a static state;
or if the angular velocity standard deviation is not less than a second angular velocity standard deviation threshold and not more than a first angular velocity standard deviation threshold, the acceleration standard deviation is not less than a second acceleration standard deviation threshold and not more than a first acceleration standard deviation threshold, and the vehicle speed is equal to 0, determining that the vehicle is in a stationary state;
or if the engine speed is less than a preset engine speed threshold value and the vehicle speed is equal to 0, determining that the vehicle is in a static state.
Optionally, the positioning the vehicle based on the vehicle state after the secondary correction includes:
if the vehicle can be connected to a Global Positioning System (GPS) server at present, acquiring a GPS positioning position, a GPS three-dimensional speed and a positioning accuracy factor of the vehicle through the GPS server;
performing combined filtering on the vehicle state after the secondary correction based on the GPS positioning position, the GPS three-dimensional speed and the positioning accuracy factor;
and positioning the vehicle based on the vehicle state obtained after the combined filtering.
A second aspect of the present application provides a vehicle positioning apparatus comprising:
the acquisition unit is used for acquiring the vehicle state after the primary correction, wherein the vehicle state comprises speed, position, direction and posture;
the detection unit is used for detecting the motion state of the vehicle in real time;
a virtual constraint correcting unit configured to perform secondary correction on the vehicle state after the primary correction based on virtual constraint correction when it is detected that the vehicle is in a traveling state;
a zero-speed correction unit configured to perform secondary correction on the vehicle state after the primary correction based on zero-speed correction when it is detected that the vehicle is in a stationary state;
and the positioning unit is used for positioning the vehicle based on the vehicle state after the secondary correction.
Optionally, the obtaining unit includes:
the first initialization subunit is used for carrying out online calibration and error compensation on an inertial sensor of the vehicle in a static state of the vehicle so as to realize real-time correction of output data of a gyroscope and an accelerometer in the inertial sensor and finish initialization of zero offset data of the gyroscope;
the second initialization subunit is used for carrying out initial alignment on the inertial sensor in a straight-line driving state of the vehicle so as to realize initialization of each parameter in the integrated navigation module;
and the vehicle state updating subunit is used for updating the vehicle state according to the output data of the gyroscope and the accelerometer which are corrected in real time so as to obtain the vehicle state after primary correction.
Optionally, the detection unit includes:
an angular velocity standard deviation obtaining subunit, configured to obtain a standard deviation of output data of the gyroscope corrected in real time within a preset time, and record the standard deviation as an angular velocity standard deviation;
the acceleration standard deviation acquisition subunit is used for acquiring the standard deviation of the output data of the accelerometer corrected in real time within a preset time, and recording the standard deviation as the acceleration standard deviation;
an engine speed acquisition subunit for acquiring an engine speed;
the vehicle speed obtaining subunit is used for obtaining a vehicle speed;
and the motion state detection subunit is used for detecting the motion state of the vehicle in real time according to the angular speed standard deviation, the acceleration standard deviation, the engine rotating speed and/or the vehicle speed.
Optionally, the motion state detection subunit includes:
and a running state determining subunit, configured to determine that the vehicle is in a running state if the angular speed standard deviation is greater than a preset first angular speed standard deviation threshold, the acceleration standard deviation is greater than a preset first acceleration standard deviation threshold, and the vehicle speed is greater than 0.
Optionally, the motion state detection subunit further includes:
a stationary state determining subunit, configured to determine that the vehicle is in a stationary state if the angular velocity standard deviation is not greater than a preset second angular velocity standard deviation threshold and the acceleration standard deviation is not greater than a preset second acceleration standard deviation threshold; or if the angular velocity standard deviation is not less than a second angular velocity standard deviation threshold and not more than a first angular velocity standard deviation threshold, the acceleration standard deviation is not less than a second acceleration standard deviation threshold and not more than a first acceleration standard deviation threshold, and the vehicle speed is equal to 0, determining that the vehicle is in a stationary state; or if the engine speed is less than a preset engine speed threshold value and the vehicle speed is equal to 0, determining that the vehicle is in a static state.
Optionally, the positioning unit includes:
a GPS information acquisition subunit, configured to, if the vehicle can be currently connected to a GPS server, acquire a GPS positioning position, a GPS three-dimensional speed, and a positioning accuracy factor of the vehicle through the GPS server;
a combined filtering subunit, configured to perform combined filtering on the vehicle state after the secondary correction based on the GPS positioning position, the GPS three-dimensional velocity, and the positioning accuracy factor;
and the vehicle positioning subunit is used for positioning the vehicle based on the vehicle state obtained after the combined filtering.
A third aspect of the present application provides a terminal device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method according to the first aspect when executing the computer program.
A fourth aspect of the present application provides a computer-readable storage medium storing a computer program which, when executed by a processor, performs the steps of the method of the first aspect and any one of the alternatives above.
A fifth aspect of the present application provides a computer program product comprising a computer program that, when executed by one or more processors, performs the steps of the method of the first aspect and any one of the alternatives described above.
As can be seen from the above, according to the present application, a vehicle state after primary correction is first obtained, where the vehicle state includes a speed, a position, an orientation, and a posture, then a motion state of a vehicle is detected in real time, when it is detected that the vehicle is in a driving state, the vehicle state after primary correction is secondarily corrected based on virtual constraint correction, when it is detected that the vehicle is in a stationary state, the vehicle state after primary correction is secondarily corrected based on zero-speed correction, and finally the vehicle is positioned based on the vehicle state after secondary correction. According to the scheme, on the premise of accurately judging the running state of the vehicle, parameters such as speed, position, direction and posture of the vehicle are correspondingly corrected according to different motion states of the vehicle, and the accuracy of vehicle positioning can be improved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings used in the embodiments or the description of the prior art will be briefly described below.
FIG. 1 is a schematic flow chart illustrating an implementation of a vehicle positioning method provided in an embodiment of the present application;
FIG. 2 is a schematic diagram of a coordinate transformation relationship in a vehicle positioning method provided in an embodiment of the present application;
FIG. 3 is a block diagram of a vehicle positioning device according to an embodiment of the present disclosure;
fig. 4 is a schematic diagram of a terminal device provided in an embodiment of the present application.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
In order to explain the technical solution of the present application, the following description will be given by way of specific examples.
Example one
The following describes a vehicle positioning method provided in an embodiment of the present application. Referring to fig. 1, a vehicle positioning method in an embodiment of the present application includes:
in step 101, a vehicle state after primary correction is acquired;
in the embodiment of the application, the vehicle state after the vehicle is corrected once is firstly obtained, wherein the vehicle state comprises the speed, the position, the direction and the posture of the vehicle. Optionally, the step 101 includes:
step A1, in a static state of the vehicle, performing online calibration and error compensation on an inertial sensor of the vehicle to realize real-time correction of output data of a gyroscope and an accelerometer in the inertial sensor and finish initialization of zero offset data of the gyroscope;
step A2, in the straight-line driving state of the vehicle, the inertial sensor is initially aligned to realize the initialization of each parameter in the integrated navigation module;
when the vehicle is in a straight-line driving state, if an effective positioning result returned by a Global Positioning System (GPS) is received and the speed returned by the GPS is not less than a preset speed measurement threshold, performing initial alignment on the inertial sensor to realize initialization of each parameter in the GPS/INS integrated navigation module, wherein the parameters contained in the GPS/INS integrated navigation module include: position, velocity, attitude, zero offset estimate of the gyroscope, and zero offset estimate of the accelerometer. The effective positioning result may be determined from a flag bit of returned GPS data, and the preset speed measurement threshold is an empirical value, and may be set to 5m/s, for example, or other values, which is not limited herein. Specifically, the specific manner of the initial alignment is determined according to the type of the inertial sensor, and is not limited herein.
And step A3, updating the vehicle state according to the real-time corrected output data of the gyroscope and the accelerometer to obtain the vehicle state after primary correction.
In step 102, detecting the motion state of the vehicle in real time;
in the embodiment of the present application, the moving state of the vehicle includes a driving state and a stationary state. The vehicle state after the primary correction obtained in the above step 101 is subjected to the secondary correction in different manners for different motion states of the vehicle. Optionally, in order to guarantee the detection accuracy of the motion state of the vehicle, the step 102 includes:
step B1, acquiring the standard deviation of the output data of the gyroscope corrected in real time within the preset time, and recording the standard deviation as the angular velocity standard deviation;
step B2, acquiring the standard deviation of the output data of the accelerometer corrected in real time within the preset time, and recording the standard deviation as the acceleration standard deviation;
wherein the preset time may be set to 1 second. Therefore, the angular velocity standard deviation within 1 second and the acceleration standard deviation within 1 second can be obtained by the above steps B1 and B2. Of course, the preset time may be changed by a user or a system, and is not limited herein.
Step B3, obtaining the rotation speed and the speed of the engine;
the engine speed and the vehicle speed may be obtained by a vehicle diagnostic system OBD, or may also be obtained by a vehicle central control instrument, or of course, other obtaining manners may also be available, which is not limited herein.
And step B4, detecting the motion state of the vehicle in real time according to the angular velocity standard deviation, the acceleration standard deviation, the engine speed and/or the vehicle speed.
In the embodiment of the application, the motion state of the vehicle is not judged by the vehicle speed; in fact, in order to reduce the influence of the output noise of the inertial sensor and the engine shake on the motion state judgment result, the limitation of the vehicle speed and the engine speed is added in the embodiment of the present application, that is, the real-time detection of the motion state of the vehicle is realized through the angular speed standard deviation, the acceleration standard deviation, the engine speed and/or the vehicle speed. Specifically, when the angular velocity standard deviation is represented by std _ Gyro, the acceleration standard deviation is represented by std _ Acc, the engine speed is represented by rpm, and the vehicle speed is represented by vel, the determination of the motion state of the vehicle is as follows:
wherein, the gyrothe 1 is a first angular velocity standard deviation threshold, the gyrothe 2 is a second angular velocity standard deviation threshold, the accthru 1 is a first acceleration standard deviation threshold, and the accthru 2 is a second acceleration standard deviation threshold; the above table can be expressed as:
if the angular speed standard deviation is greater than a preset first angular speed standard deviation threshold value, the acceleration standard deviation is greater than a preset first acceleration standard deviation threshold value, and the vehicle speed is greater than 0, determining that the vehicle is in a running state;
if the angular velocity standard deviation is not larger than a preset second angular velocity standard deviation threshold value, and the acceleration standard deviation is not larger than a preset second acceleration standard deviation threshold value, determining that the vehicle is in a static state; or if the angular velocity standard deviation is not less than a second angular velocity standard deviation threshold and not more than a first angular velocity standard deviation threshold, the acceleration standard deviation is not less than a second acceleration standard deviation threshold and not more than a first acceleration standard deviation threshold, and the vehicle speed is equal to 0, determining that the vehicle is in a stationary state; or if the engine speed is less than a preset engine speed threshold value and the vehicle speed is equal to 0, determining that the vehicle is in a static state.
Alternatively, the first angular velocity standard deviation threshold, the second angular velocity standard deviation threshold, the first acceleration standard deviation threshold, and the second acceleration standard deviation threshold may be appropriately relaxed to increase the accuracy of the determination of the motion state of the vehicle. The values of the thresholds also depend on the application scenario of the inertial sensor, the output of the inertial sensor itself, the type of the inertial sensor, and the manufacturing process of the inertial sensor. Specifically, for a consumer Micro-Electro-mechanical system (MEMS) 6-axis or 9-axis inertial sensor, it is required that output data of three axes of a gyroscope and an accelerometer satisfy determination conditions in the above table to determine the motion state of the vehicle.
Optionally, although the first angular velocity standard deviation threshold, the second angular velocity standard deviation threshold, the first acceleration standard deviation threshold, and the second acceleration standard deviation threshold are all preset thresholds, in order to enable the four thresholds to meet an actual application scenario, in step a1 performed before, each output data of the inertial sensor when the current vehicle is stationary may be obtained first, that is, the acceleration standard deviation of the accelerometer and the angular velocity standard deviation of the gyroscope when the current vehicle is stationary are obtained and compared with the determination conditions in the table, and if a mismatch is found, the threshold associated with the mismatch determination condition is adjusted, so that the value of each threshold can meet the determination accuracy.
In step 103, when it is detected that the vehicle is in a traveling state, performing secondary correction on the vehicle state after the primary correction based on virtual constraint correction;
in step 104, when the vehicle is detected to be in a stationary state, performing a secondary correction on the vehicle state after the primary correction based on a zero-speed correction;
in step 105, the vehicle is positioned based on the vehicle state after the secondary correction.
In the embodiment of the present application, the accuracy of the vehicle state after the secondary correction is improved to a certain extent compared to the vehicle state after the primary correction in step 101, and therefore, the vehicle can be positioned based on the vehicle state after the secondary correction. Optionally, since the correction processes of the first correction and the second correction are both performed by using parameters inside the vehicle, some estimation errors still exist in the correction results. Thus, ambient-related parameters may be introduced to eliminate these estimation errors. Based on this, the above step 105 may include:
step C1, detecting whether the current can be connected with a GPS server;
step C2, if the vehicle can be connected to the GPS server at present, acquiring the GPS positioning position, the GPS three-dimensional speed and the positioning accuracy factor of the vehicle through the GPS server;
step C3, based on the GPS positioning position, the GPS three-dimensional speed and the positioning accuracy factor, performing combined filtering on the vehicle state after the secondary correction;
and step C4, positioning the vehicle based on the vehicle state obtained after the combined filtering.
The GPS server may acquire GPS information of the vehicle. Because the GPS information is obtained through satellite positioning and does not belong to parameters in the vehicle, the estimation error left after the previous correction and the secondary correction can be eliminated to a certain extent by introducing the GPS information such as the GPS positioning position, the GPS three-dimensional speed, the positioning accuracy factor and the like. Of course, when the vehicle cannot be connected to the GPS server, for example, when the vehicle enters an environment such as a tunnel or the ground, the vehicle may not be connected to the GPS server, and in such cases, the vehicle may be subjected to a short-time positioning operation based only on the vehicle state obtained after the secondary correction.
Specifically, the virtual constraint modification in step 103 is implemented as follows:
d1, modeling errors of the GPS/INS integrated navigation system, wherein the GPS/INS integrated navigation system is mounted in a GPS/INS integrated navigation module, and can select a position, a speed, an attitude, an azimuth angle, a gyroscope constant zero offset and an accelerometer constant zero offset as state variables, wherein the position comprises a latitude, a longitude and an elevation, the speed comprises a northbound speed, an easbound speed and a vertical speed, the attitude comprises a roll angle and a pitch angle, the gyroscope constant zero offset comprises a northbound gyroscope zero offset, an easbound gyroscope zero offset and a vertical gyroscope zero offset, and the accelerometer constant zero offset comprises a northbound accelerometer zero offset, an easbound accelerometer zero offset and a vertical accelerometer zero offset. That is, the state variables include fifteen states, which are latitude, longitude, elevation, northbound speed, eastern speed, vertical speed, roll angle, pitch angle, azimuth angle, zero offset of northbound gyroscope, zero offset of eastern gyroscope, zero offset of vertical gyroscope, zero offset of northbound accelerometer, zero offset of eastern accelerometer, and zero offset of vertical accelerometer. After the state variables are obtained, the position, speed and attitude updating equations of an INS inertial navigation system in the GPS/INS integrated navigation system are as follows:
in the above formula (1), rn、vnRespectively a position vector and a velocity vector in a navigation coordinate system O-xnynznI.e., projection in the NorthEast (NED) coordinate system;as a carrier coordinate system O-xbybzbTo the navigation coordinate system O-xnynznThe coordinate transformation matrix of (2); f. ofbIs the output of an accelerometer in an inertial sensor, i.e. the specific force outputIs the earth center earth fixed coordinate system O-xeyezeRelative to an inertial frame O-xiyiziI.e., the projection of the angular rate of rotation of the earth in the navigational coordinate system, and, in particular,whereinIs the angular rate of rotation of the earth,is the geographic latitude;is a projection of the rotation of the navigational coordinate system with respect to the terrestrial coordinate system under the navigational coordinate system,h is the altitude; gnIs the projection of the earth gravity acceleration in a navigation coordinate system, gn＝[0 0 gc]T，gcThe gravity acceleration of the current position of the vehicle is obtained;for the output of gyroscopes in inertial sensors, i.e. angular velocity outputIs a projection of the rotation of the navigation coordinate system relative to the inertial coordinate system in the carrier coordinate system,d in the above formula is specifically:
wherein Rm meridian curvature radius and Rn is prime circle curvature radius.
D2, carrying out disturbance analysis on the c system, and omitting higher terms of more than two orders to obtain a state equation of the GPS/INS combined navigation system error model as follows:
wherein, δ rc、δvcRespectively, a position error vector and a velocity error vector, phi is an attitude angle error,is a coordinate transformation matrix from the carrier coordinate system to the platform coordinate system, specifically,
d3, when the vehicle is in a driving state,
in the above formula (3), vb、vnRespectively a vehicle carrier coordinate system and a speed vector under a navigation coordinate system,respectively outputting lateral and ground speed of a vehicle carrier coordinate system; the disturbance analysis is carried out on the formula (3) to establish the view of virtual constraint correctionMeasuring a model;
in the above formula (4), (E)nX) is composed of ∈n＝[εNεEεD]TFormed cross-product antisymmetric matrices, in particularWherein epsilonN、εE、εDFor the attitude angle error, the second order small term is omitted from equation (4) above, and the following can be obtained:
selectingAs observed quantities, the corresponding observation models are:
wherein, CijIs composed ofI row and j column elements of vN、vE、vDOutputting the speed of the vehicle carrier in the north direction, the east direction and the ground direction under a navigation coordinate system;
d4, performing Kalman filtering processing on the models obtained by the D2 and the D3, and performing real-time feedback on the state error estimated value, so that the divergence of the horizontal position and the carrier azimuth angle of the GPS/INS integrated navigation system can be effectively inhibited.
In the above steps D1 to D4, kalman filtering, state feedback correction, and the correlation between the platform coordinate system p, the navigation coordinate system n, and the calculation coordinate system c are involved. For a more clear explanation of the above steps D1 to D4, the relationship between coordinate systems, kalman filtering, and state feedback correction are described as follows:
the relationship among the platform coordinate system p, the navigation coordinate system n and the calculation coordinate system c is shown in the attached figure 2:
in the context of the attached figure 2,and isIn the same way, the method for preparing the composite material,omitting the second order small term, the method can be approximated to obtain: phi ≈ psi + delta theta
For kalman filtering, its time updates are:
the measurement is updated as follows:
wherein,predicting an estimate, phi, for the state at time kk,k-1Is the state transition matrix from time k-1 to time k, Pk,k-1For the covariance matrix prediction, Q, of the state at time kk-1Covariance matrix of system noise at time K-1, KkGain matrix for time k, RkIs the measured noise covariance matrix at time k, zkIs a measured value at the time k,is a filtered estimate of time k, PkThe estimate is filtered for the state covariance matrix at time k. When the GPS/INS integrated navigation system selects delta rn、δvn、δεnWhen the state variable is a state variable, after the Kalman filtering is finished, the state can be fed back to the strapdown inertial navigation output in real time, and a state estimation valueTo be cleared, the above equation can be simplified to:
for the state feedback correction, the position feedback is as follows:
the speed feedback is as follows:
the posture feedback is as follows:
wherein,is composed of epsilonn＝[εNεEεD]TForm cross multiplication inverseA symmetric matrix.
As can be seen from the above, according to the embodiment of the present application, after the vehicle state after the primary correction is acquired, the motion state of the vehicle is detected in real time through the acquired output data of the engine speed, the vehicle speed, the gyroscope of the inertial sensor, and the acceleration, so as to improve the accuracy of the vehicle motion determination, and when the vehicle is detected to be in the driving state, the vehicle state after the primary correction is corrected secondarily based on the virtual constraint correction, when the vehicle is detected to be in the stationary state, the vehicle state after the primary correction is corrected secondarily based on the zero speed correction, and finally, the vehicle is positioned based on the vehicle state after the secondary correction. The virtual constraint correction process combines the motion characteristics of the vehicle carrier, simultaneously ignores the influence of the rotation angular rate of the earth on the output of the gyroscope, and can effectively inhibit the positioning drift of the integrated navigation system in the shielding environment of underground garages, tunnels and the like.
It should be understood that, the sequence numbers of the steps in the foregoing embodiments do not imply an execution sequence, and the execution sequence of each process should be determined by its function and inherent logic, and should not constitute any limitation to the implementation process of the embodiments of the present application.
Example two
The present embodiment provides a vehicle positioning apparatus, as shown in fig. 3, a vehicle positioning apparatus 300 in the present embodiment includes:
an obtaining unit 301, configured to obtain a vehicle state after primary correction, where the vehicle state includes a speed, a position, an orientation, and a posture;
a detection unit 302 for detecting a motion state of the vehicle in real time;
a virtual constraint correcting unit 303 configured to perform secondary correction on the vehicle state after the primary correction based on virtual constraint correction when it is detected that the vehicle is in a traveling state;
a stall correction unit 304 configured to perform a secondary correction on the vehicle state after the primary correction based on a stall correction when it is detected that the vehicle is in a stationary state;
and a positioning unit 305 for positioning the vehicle based on the vehicle state after the second correction.
Optionally, the obtaining unit 301 includes:
the first initialization subunit is used for carrying out online calibration and error compensation on an inertial sensor of the vehicle in a static state of the vehicle so as to realize real-time correction of output data of a gyroscope and an accelerometer in the inertial sensor and finish initialization of zero offset data of the gyroscope;
the second initialization subunit is used for carrying out initial alignment on the inertial sensor in a straight-line driving state of the vehicle so as to realize initialization of each parameter in the integrated navigation module;
and the vehicle state updating subunit is used for updating the vehicle state according to the output data of the gyroscope and the accelerometer which are corrected in real time so as to obtain the vehicle state after primary correction.
Optionally, the detecting unit 302 includes:
an angular velocity standard deviation obtaining subunit, configured to obtain a standard deviation of output data of the gyroscope corrected in real time within a preset time, and record the standard deviation as an angular velocity standard deviation;
the acceleration standard deviation acquisition subunit is used for acquiring the standard deviation of the output data of the accelerometer corrected in real time within a preset time, and recording the standard deviation as the acceleration standard deviation;
an engine speed acquisition subunit for acquiring an engine speed;
the vehicle speed obtaining subunit is used for obtaining a vehicle speed;
and the motion state detection subunit is used for detecting the motion state of the vehicle in real time according to the angular speed standard deviation, the acceleration standard deviation, the engine rotating speed and/or the vehicle speed.
Optionally, the motion state detection subunit includes:
and a running state determining subunit, configured to determine that the vehicle is in a running state if the angular speed standard deviation is greater than a preset first angular speed standard deviation threshold, the acceleration standard deviation is greater than a preset first acceleration standard deviation threshold, and the vehicle speed is greater than 0.
Optionally, the motion state detection subunit further includes:
a stationary state determining subunit, configured to determine that the vehicle is in a stationary state if the angular velocity standard deviation is not greater than a preset second angular velocity standard deviation threshold and the acceleration standard deviation is not greater than a preset second acceleration standard deviation threshold; or if the angular velocity standard deviation is not less than a second angular velocity standard deviation threshold and not more than a first angular velocity standard deviation threshold, the acceleration standard deviation is not less than a second acceleration standard deviation threshold and not more than a first acceleration standard deviation threshold, and the vehicle speed is equal to 0, determining that the vehicle is in a stationary state; or if the engine speed is less than a preset engine speed threshold value and the vehicle speed is equal to 0, determining that the vehicle is in a static state.
Optionally, the positioning unit 305 includes:
a GPS information acquisition subunit, configured to, if the vehicle can be currently connected to a GPS server, acquire a GPS positioning position, a GPS three-dimensional speed, and a positioning accuracy factor of the vehicle through the GPS server;
a combined filtering subunit, configured to perform combined filtering on the vehicle state after the secondary correction based on the GPS positioning position, the GPS three-dimensional velocity, and the positioning accuracy factor;
and the vehicle positioning subunit is used for positioning the vehicle based on the vehicle state obtained after the combined filtering.
As can be seen from the above, according to the embodiment of the present application, after the vehicle positioning apparatus obtains the vehicle state after the primary correction, the vehicle positioning apparatus detects the motion state of the vehicle in real time through the obtained output data of the engine speed, the vehicle speed, the gyroscope of the inertial sensor, and the acceleration, so as to improve the accuracy of the vehicle motion determination, and when it is detected that the vehicle is in the driving state, performs the secondary correction on the vehicle state after the primary correction based on the virtual constraint correction, when it is detected that the vehicle is in the stationary state, performs the secondary correction on the vehicle state after the primary correction based on the zero speed correction, and finally positions the vehicle based on the vehicle state after the secondary correction. The virtual constraint correction process combines the motion characteristics of the vehicle carrier, simultaneously ignores the influence of the rotation angular rate of the earth on the output of the gyroscope, and can effectively inhibit the positioning drift of the integrated navigation system in the shielding environment of underground garages, tunnels and the like.
EXAMPLE III
Fig. 4 shows a schematic structural diagram of a terminal device provided in an embodiment of the present application, where the terminal device 4 of the embodiment includes: a processor 40, a memory 41 and a computer program 42 stored in the memory 41 and operable on the processor 40, such as a vehicle positioning program in a vehicle positioning method. The processor 40, when executing the computer program 42, implements the steps of the various embodiments of the vehicle localization method described above, such as the steps S101 to 105 shown in fig. 1.
The terminal device 4 may include, but is not limited to, a processor 40 and a memory 41. It will be understood by those skilled in the art that fig. 4 is only an example of the terminal device 4, and does not constitute a limitation to the terminal device 4, and may include more or less components than those shown, or combine some components, or different components, for example, the terminal device 4 may further include a network access device, etc.
The Processor 40 may be a Central Processing Unit (CPU), other general-purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA) or other Programmable logic device, a discrete Gate or transistor logic device, a discrete hardware component, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The storage 41 may be an internal storage module of the terminal device 4, such as a hard disk or a memory of the terminal device 4. The memory 41 may be an external storage device of the terminal device 4, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), or the like provided in the terminal device 4. Further, the memory 41 may include both an internal storage module and an external storage device of the terminal device 4. The memory 41 is used to store the computer program and other programs and data required by the terminal device 4. The above-mentioned memory 41 may also be used to temporarily store data that has been output or is to be output.
Illustratively, when the terminal device 4 performs vehicle positioning, the processor 40 implements the following steps by executing the computer program stored in the memory 41:
acquiring a vehicle state after primary correction, wherein the vehicle state comprises speed, position, direction and posture;
detecting the motion state of the vehicle in real time;
when the vehicle is detected to be in a running state, performing secondary correction on the vehicle state after the primary correction based on virtual constraint correction;
when the vehicle is detected to be in a static state, performing secondary correction on the vehicle state after the primary correction based on zero-speed correction;
and positioning the vehicle based on the vehicle state after the secondary correction.
Optionally, the obtaining the vehicle state after the primary correction includes:
in a static state of a vehicle, carrying out online calibration and error compensation on an inertial sensor of the vehicle so as to realize real-time correction of output data of a gyroscope and an accelerometer in the inertial sensor and finish initialization of zero offset data of the gyroscope;
in the straight-line running state of the vehicle, the inertial sensors are initially aligned to realize the initialization of each parameter in the integrated navigation module;
and updating the vehicle state according to the output data of the gyroscope and the accelerometer which are corrected in real time to obtain the vehicle state after primary correction.
Optionally, the detecting the motion state of the vehicle in real time includes:
acquiring a standard deviation of output data of the gyroscope corrected in real time within a preset time, and recording the standard deviation as an angular velocity standard deviation;
acquiring a standard deviation of output data of the accelerometer corrected in real time within a preset time, and recording the standard deviation as an acceleration standard deviation;
acquiring the rotating speed and the vehicle speed of an engine;
and detecting the motion state of the vehicle in real time according to the angular speed standard deviation, the acceleration standard deviation, the engine rotating speed and/or the vehicle speed.
Optionally, the detecting a motion state of the vehicle in real time according to the angular velocity standard deviation, the acceleration standard deviation, the engine speed, and the vehicle speed includes:
and if the angular speed standard deviation is greater than a preset first angular speed standard deviation threshold value, the acceleration standard deviation is greater than a preset first acceleration standard deviation threshold value, and the vehicle speed is greater than 0, determining that the vehicle is in a running state.
Optionally, the detecting a motion state of the vehicle in real time according to the angular velocity standard deviation, the acceleration standard deviation, the engine speed, and the vehicle speed includes:
if the angular velocity standard deviation is not larger than a preset second angular velocity standard deviation threshold value, and the acceleration standard deviation is not larger than a preset second acceleration standard deviation threshold value, determining that the vehicle is in a static state;
or if the angular velocity standard deviation is not less than a second angular velocity standard deviation threshold and not more than a first angular velocity standard deviation threshold, the acceleration standard deviation is not less than a second acceleration standard deviation threshold and not more than a first acceleration standard deviation threshold, and the vehicle speed is equal to 0, determining that the vehicle is in a stationary state;
or if the engine speed is less than a preset engine speed threshold value and the vehicle speed is equal to 0, determining that the vehicle is in a static state.
Optionally, the positioning the vehicle based on the vehicle state after the secondary correction includes:
if the vehicle can be connected to a Global Positioning System (GPS) server at present, acquiring a GPS positioning position, a GPS three-dimensional speed and a positioning accuracy factor of the vehicle through the GPS server;
performing combined filtering on the vehicle state after the secondary correction based on the GPS positioning position, the GPS three-dimensional speed and the positioning accuracy factor;
and positioning the vehicle based on the vehicle state obtained after the combined filtering.
Illustratively, the computer program 42 may be divided into one or more units, and the one or more units are stored in the memory 41 and executed by the processor 40 to complete the present application.
As can be seen from the above, according to the embodiment of the present application, after acquiring the vehicle state after the primary correction, the terminal device detects the motion state of the vehicle in real time through the acquired output data of the engine speed, the vehicle speed, the gyroscope of the inertial sensor, and the acceleration, so as to improve the accuracy of vehicle motion determination, and when detecting that the vehicle is in a driving state, performs the secondary correction on the vehicle state after the primary correction based on the virtual constraint correction, when detecting that the vehicle is in a stationary state, performs the secondary correction on the vehicle state after the primary correction based on the zero speed correction, and finally positions the vehicle based on the vehicle state after the secondary correction. The virtual constraint correction process combines the motion characteristics of the vehicle carrier, simultaneously ignores the influence of the rotation angular rate of the earth on the output of the gyroscope, and can effectively inhibit the positioning drift of the integrated navigation system in the shielding environment of underground garages, tunnels and the like.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other ways. For example, the above-described embodiments of the apparatus/terminal device are merely illustrative, and for example, the division of the above modules or units is only one logical function division, and there may be other division manners in actual implementation, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated unit may be stored in a computer-readable storage medium if it is implemented in the form of a software functional unit and sold or used as a separate product. Based on such understanding, all or part of the flow in the method of the embodiments described above may be implemented by a computer program, which may be stored in a computer readable storage medium and used by a processor to implement the steps of the embodiments of the methods described above. . The computer program includes computer program code, and the computer program code may be in a source code form, an object code form, an executable file or some intermediate form. The computer readable medium may include: any entity or device capable of carrying the above-mentioned computer program code, recording medium, usb disk, removable hard disk, magnetic disk, optical disk, computer Memory, Read-Only Memory (ROM), Random Access Memory (RAM), electrical carrier signal, telecommunication signal, software distribution medium, etc. It should be noted that the computer readable medium described above may be suitably increased or decreased as required by legislation and patent practice in jurisdictions, for example, in some jurisdictions, computer readable media excludes electrical carrier signals and telecommunications signals in accordance with legislation and patent practice.
The above-mentioned embodiments are only used for illustrating the technical solutions of the present application, and not for limiting the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not substantially depart from the spirit and scope of the embodiments of the present application and are intended to be included within the scope of the present application.