A three-dimensional vehicle positioning method and system

Abstract

An embodiment of the present application provides a three-dimensional vehicle positioning method and system. The three-dimensional state observation quantity of the target vehicle is obtained through the target vehicle positioning system. If it is determined that the three-dimensional state observation quantity meets the stability condition, the Kalman filter in the positioning system is initialized according to the three-dimensional state observation quantity. If the target vehicle is in a moving state, the three-dimensional state of the target vehicle is determined through the initialized Kalman filter according to the three-dimensional state observation quantity. Since the positioning system includes a vehicle odometer, an inertial measurement unit, a barometer, and a satellite positioning module, the Kalman filter is used to fuse and solve the measurement data obtained by these multiple sensors, improving the reliability of the positioning system and also improving the accuracy of three-dimensional positioning of the vehicle, enabling the vehicle to have better positioning accuracy in scenarios such as navigation that require altitude position information, thereby improving the user driving experience.

Description

Technical Field

[0001] This application relates to the technical field of vehicle positioning, and particularly to a vehicle three-dimensional positioning method and system. Background Art

[0002] Vehicle three-dimensional positioning refers to estimating the position of a vehicle in a three-dimensional space, which can monitor the motion state of the vehicle and can be applied to a vehicle navigation system to guide a user's driving.

[0003] In related technologies, vehicle navigation systems generally use the motion information of a vehicle in a two-dimensional plane to position and navigate the vehicle. They focus on the motion information of the vehicle in the horizontal plane direction and ignore the motion information of the vehicle in the vertical (height) direction. As a result, the positioning accuracy of the vehicle is not high in some scenarios. For example, when a vehicle is driving in an overpass or viaduct area, the vehicle navigation system cannot distinguish whether it is on the bridge or under the bridge; when a vehicle is in a multi-layer stereoscopic parking garage, the vehicle navigation system cannot distinguish the floor of the parking lot where the vehicle is located.

[0004] The above vehicle positioning technology makes it difficult to achieve lane-level high-precision positioning of the vehicle due to inaccurate or missing motion information in the height direction, and the user driving experience is poor. Summary of the Invention

[0005] In order to solve the above technical problems existing in the prior art, this application provides a vehicle three-dimensional positioning method and system, which improves the vehicle three-dimensional positioning accuracy.

[0006] On the one hand, an embodiment of this application provides a vehicle three-dimensional positioning method, and the method includes:

[0007] Obtain three-dimensional state observables of the target vehicle through a positioning system; the positioning system includes a vehicle odometer, an inertial measurement unit IMU, a barometer, and a satellite positioning module;

[0008] If it is determined that the three-dimensional state observables meet the stability conditions, initialize a Kalman filter in the positioning system according to the three-dimensional state observables;

[0009] If it is determined that the target vehicle is in a motion state, determine the three-dimensional state of the target vehicle through the initialized Kalman filter according to the three-dimensional state observables.

[0010] In a possible implementation manner, the three-dimensional state observables include three-dimensional attitude observables, and the three-dimensional attitude observables include a heading angle, a roll angle, and a pitch angle. The obtaining of the three-dimensional state observables of the target vehicle through the positioning system includes:

[0011] Obtain the speed information of the target vehicle through the vehicle odometer, obtain the specific force information of the target vehicle through the accelerometer in the IMU, and obtain the angular velocity information of the target vehicle through the gyroscope in the IMU;

[0012] Filter the speed information, the specific force information, and the angular velocity information;

[0013] According to the filtered speed information and the filtered angular velocity information, correct the filtered specific force information to obtain the three-axis gravitational acceleration of the target vehicle;

[0014] Calculate the roll angle and pitch angle of the target vehicle according to the three-axis gravitational acceleration;

[0015] Obtain the dual-antenna heading angle of the target vehicle through the satellite positioning module.

[0016] In a possible implementation manner, the correcting the filtered specific force information according to the filtered speed information to obtain the three-axis gravitational acceleration of the target vehicle includes:

[0017] Determine the forward linear acceleration of the target vehicle according to the speed information;

[0018] Determine the centripetal acceleration of the target vehicle according to the speed information and the angular velocity of the target vehicle's heading axis;

[0019] Determine the forward gravitational acceleration of the target vehicle according to the forward linear acceleration and the forward specific force obtained through the accelerometer;

[0020] Determine the lateral gravitational acceleration of the target vehicle according to the centripetal acceleration and the lateral specific force obtained through the accelerometer.

[0021] In a possible implementation manner, the three-dimensional state observation quantity includes a three-dimensional position observation quantity, and the obtaining the three-dimensional position observation quantity of the target vehicle through the positioning system includes:

[0022] Obtain the three-dimensional position information of the target vehicle through the satellite positioning module; wherein, the fixed solution and floating-point solution included in the three-dimensional position information are used as the three-dimensional position observation quantity;

[0023] Obtain the relative height position information of the target vehicle through the barometer; the relative height position information is used as the three-dimensional position observation quantity and is determined based on the fixed solution.

[0024] In a possible implementation manner, the method further includes:

[0025] Perform a validity check on the relative height position information to obtain a first check result;

[0026] If the first check result indicates that the relative height position information is valid, then use the relative height position information as the height position observation of the target vehicle.

[0027] In a possible implementation, the method further includes:

[0028] If the height position information obtained by the satellite positioning module meets the availability condition, and the relative height position information obtained by the barometer does not meet the availability condition, then perform a validity check on the floating-point solution height position information obtained by the satellite positioning module to obtain a second check result;

[0029] If the second check result indicates that the floating-point solution is valid, then use the floating-point solution height position information as the height position observation of the target vehicle.

[0030] In a possible implementation, the method further includes:

[0031] If the height position information obtained by the satellite positioning module meets the availability condition, and the relative height position information obtained by the barometer meets the availability condition, determine whether the height position information includes a fixed solution;

[0032] If so, use the fixed solution height position information as the height position observation of the target vehicle;

[0033] If not, subtract the height position information obtained by the satellite positioning module and the relative height position information obtained by the barometer from the Kalman state recursive prediction height respectively to obtain a first difference corresponding to the satellite positioning module and a second difference corresponding to the barometer;

[0034] If the first difference is less than the second difference, use the height position information obtained by the satellite positioning module as the height position observation of the target vehicle;

[0035] If the first difference is not less than the second difference, use the relative height position information obtained by the barometer as the height position observation of the target vehicle.

[0036] In a possible implementation, obtaining the three-dimensional speed observation of the target vehicle through the target vehicle positioning system includes:

[0037] Obtain the three-dimensional speed information of the target vehicle through the satellite positioning module, and obtain the speed information of the target vehicle through the vehicle odometer;

[0038] If the ground speed of the satellite positioning module is greater than a first threshold and the speed of the target vehicle in the height direction obtained through the satellite positioning module is less than a second threshold, the three-dimensional speed information corresponding to the satellite positioning module and the speed information corresponding to the vehicle odometer are used as the three-dimensional speed observation quantity of the target vehicle.

[0039] In a possible implementation manner, the method further includes:

[0040] If the time when the satellite positioning module is blocked exceeds a time threshold, the data within a preset time after the satellite positioning module is unblocked is excluded.

[0041] On the other hand, an embodiment of the present application provides a vehicle three-dimensional positioning system, which includes a vehicle odometer, an inertial measurement unit IMU, a barometer, a satellite positioning module, and a Kalman filter; the system is used to execute the vehicle three-dimensional positioning method described in the above aspect.

[0042] It can be seen from the above technical solutions that by obtaining the three-dimensional state observation quantity of the target vehicle through the target vehicle positioning system, if it is determined that the three-dimensional state observation quantity meets the stability condition, it indicates that the measurement data output by multiple sensors included in the positioning system tend to be in a stable state. Therefore, the Kalman filter in the positioning system can be initialized according to the three-dimensional state observation quantity, providing reliable measurement data for the Kalman filter to calculate the three-dimensional state of the target vehicle. When the target vehicle is in a moving state, it indicates that the three-dimensional state of the target vehicle has changed. Therefore, according to the three-dimensional state observation quantity, the three-dimensional state of the target vehicle is determined through the initialized Kalman filter. Since the positioning system includes a vehicle odometer, an inertial measurement unit, a barometer, and a satellite positioning module, the Kalman filter fuses and calculates the measurement data obtained by these multiple sensors, improving the reliability of the positioning system and also improving the accuracy of three-dimensional positioning of the vehicle, enabling the vehicle to have better positioning accuracy in scenarios such as navigation that require height position information, thereby improving the user driving experience. Description of the Drawings

[0043] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings described below are only some embodiments recorded in the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0044] Figure 1 It is a schematic flow chart of a vehicle three-dimensional positioning method provided by an embodiment of the present application;

[0045] Figure 2Schematic structural diagram of a vehicle three-dimensional positioning system provided by an embodiment of the present application. Detailed implementation manners

[0046] In order to enable those skilled in the art to better understand the solutions of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.

[0047] In the related art, due to the lack or inaccuracy of vehicle height information, the vehicle positioning accuracy is not high, affecting the user driving experience. Therefore, the embodiments of the present application provide a vehicle three-dimensional positioning method and system, improving the accuracy of vehicle three-dimensional positioning.

[0048] See Figure 1 , Figure 1 Schematic flow diagram of a vehicle three-dimensional positioning method provided by an embodiment of the present application. As Figure 1 shown, the vehicle three-dimensional positioning method includes the following steps:

[0049] S101: Obtain the three-dimensional state observation quantity of the target vehicle through the positioning system.

[0050] In the embodiments of the present application, the positioning system includes a vehicle odometer, an inertial measurement unit (IMU), a barometer, and a satellite positioning module. Among them, the IMU includes a gyroscope and an accelerometer. The gyroscope is used to measure the vehicle's three-axis attitude angle (or angular rate). The accelerometer is used to measure the vehicle's three-axis acceleration. The satellite positioning module may be a Global Positioning System (GPS), which is not limited herein.

[0051] During the positioning process, multiple sensors included in the above positioning system are used to measure the three-dimensional state of the target vehicle, and the measurement data is used as the three-dimensional state observation quantity of the target vehicle. Among them, the three-dimensional state observation quantity includes three-dimensional attitude observation quantity, three-dimensional position observation quantity, and three-dimensional velocity observation quantity.

[0052] The three-dimensional attitude observation quantity includes heading angle, roll angle, and pitch angle. In the present application, the roll angle and pitch angle of the target vehicle are obtained through the vehicle odometer and IMU, and the heading angle of the target vehicle is obtained through the satellite positioning module. Since there is noise in the measurement data obtained through the sensors, in practical applications, the measurement data can be filtered to reduce the influence of noise on the effective data and improve the positioning accuracy.

[0053] Specifically, the speed information of the target vehicle is obtained through the vehicle odometer, the specific force information of the target vehicle is obtained through the accelerometer, and the angular velocity information of the target vehicle is obtained through the gyroscope. Then, for the speed information, specific force information, and angular velocity information. During the application process, a filter can be used to filter the aforementioned measurement data, and no specific limitation is imposed here.

[0054] It can be understood that the specific force information output by the accelerometer in the IMU is the non-gravitational external force information of the target vehicle, that is, the difference between the actual acceleration of the target vehicle and gravity. Therefore, the specific force information obtained by the IMU is not the true acceleration information of the target vehicle. Especially when the vehicle is turning or accelerating / decelerating, there is a large error between the specific force information and the actual acceleration of the vehicle. Therefore, the specific force information cannot be directly used as the acceleration information of the vehicle.

[0055] Therefore, when obtaining the measurement data of the target vehicle, the specific force information obtained by the IMU is corrected. Specifically, based on the filtered speed information and filtered angular velocity information, the filtered specific force information can be corrected to obtain the three-axis gravitational acceleration of the target vehicle. Then, based on this three-axis gravitational acceleration, the roll angle and pitch angle of the target vehicle can be calculated.

[0056] In addition, the present application obtains the dual-antenna heading angle of the target vehicle through the satellite positioning module. Based on this, the roll angle, pitch angle, and dual-antenna heading angle obtained after the above filtering and correction are used as the three-dimensional attitude observables of the target vehicle to estimate the three-dimensional attitude of the target vehicle.

[0057] In the specific calculation process, the vehicle forward linear acceleration is calculated based on the speed information, and the vehicle centripetal acceleration is calculated using the speed information and the vehicle heading axis angular rate. Then, the vehicle forward gravitational acceleration component is obtained by subtracting the vehicle forward specific force from the forward linear acceleration, and the vehicle lateral gravitational acceleration component is obtained by subtracting the vehicle lateral specific force from the vehicle centripetal acceleration. The vehicle three-axis acceleration information is normalized, and the normalized vehicle three-axis gravitational acceleration components are used through the formulas asin(-a x ) and atan(a y / a z ) to calculate the pitch angle and roll angle. Where a x represents the forward gravitational acceleration, a y represents the lateral gravitational acceleration, and a z represents the vertical acceleration.

[0058] The three-dimensional position observation quantity includes the position of the target vehicle in a two-dimensional plane and also includes the position in the height direction. Since relying only on a single sensor to obtain the three-dimensional position observation quantity of the target vehicle cannot ensure continuous and effective positioning, the present application combines a satellite positioning module and a barometer to measure the three-dimensional position of the target vehicle. Specifically, the height position information of the target vehicle is obtained through the barometer, and the three-dimensional position information of the target vehicle is obtained through the satellite positioning module. Among them, the fixed solution and the floating-point solution included in the three-dimensional position information are used as the three-dimensional position observation quantity of the target vehicle. Based on the satellite positioning module and the barometer to obtain the three-dimensional position observation quantity of the target vehicle, compared with a single sensor, the reliability and accuracy of the positioning system are improved.

[0059] Generally, the positioning accuracy of the satellite positioning module is higher than that of the barometer. Therefore, when the fixed solution of the satellite positioning module is updated in the present application, the height position information included in the fixed solution is used as the reference value of the barometer, and the relative height information relative to this reference value is used as the three-dimensional position observation quantity of the target vehicle, solving the problem of the reference offset of the barometer caused by environmental factors such as temperature rise, and improving the accuracy of the height position information obtained through the barometer.

[0060] The three-dimensional speed observation quantity includes the speed of the target vehicle in the three-axis direction. The present application obtains the speed information of the target vehicle through a vehicle odometer and obtains the three-dimensional speed information of the target vehicle through a satellite positioning module. Similarly, compared with a single sensor, the reliability and accuracy of the positioning system are improved.

[0061] In an actual driving scenario, since there is a speed limit when the vehicle climbs a slope and the measurement data accuracy of the satellite positioning module is low at low speeds, the present application selects more accurate measurement data from the three-dimensional speed information obtained through the satellite positioning module and the speed information obtained through the vehicle odometer by setting thresholds. Specifically, if the ground speed of the satellite positioning module is greater than a first threshold and the speed of the target vehicle in the height direction obtained through the satellite positioning module is less than a second threshold, the three-dimensional speed information corresponding to the satellite positioning module and the speed information corresponding to the vehicle odometer are used as the three-dimensional speed observation quantity of the target vehicle.

[0062] The above-mentioned obtaining the attitude observation quantity, three-dimensional position observation quantity and three-dimensional speed observation quantity of the target vehicle through multiple sensors provides reliable basic data for subsequent using a Kalman filter to perform multi-information fusion to solve the three-dimensional attitude of the target vehicle, thereby improving the reliability and accuracy of the positioning system.

[0063] S102: If it is determined that the three-dimensional state observation quantity meets the stability condition, initialize the Kalman filter in the positioning system according to the three-dimensional state observation quantity.

[0064] Since the measurement data obtained by each sensor in the positioning system at the start time may be unstable, and the unstable measurement data has low credibility and cannot accurately locate the target vehicle. Therefore, in this application, by setting stability conditions, it is determined whether the measurement data of the sensors in the positioning system for the target vehicle, that is, the three-dimensional state observation quantity in S101 above, is credible.

[0065] If it is determined that the three-dimensional state observation quantity meets the stability conditions, it indicates that each sensor in the positioning system is in a relatively stable state, and the measurement data obtained by it is credible and can relatively accurately reflect the state of the target vehicle. In this case, the three-dimensional state observation quantity is used to initialize the Kalman filter in the positioning system. If it is determined that the three-dimensional state observation quantity does not meet the stability conditions, it indicates that the state of each sensor in the positioning system is not stable enough, and the measurement data obtained by it is not credible and is not suitable for accurately locating the target vehicle.

[0066] The Kalman filter is a high-efficiency recursive filter (autoregressive filter), which can estimate the state of a dynamic system from a series of incomplete and noisy measurements. In this application, the Kalman filter is used to perform three-dimensional positioning on the target vehicle.

[0067] During the initialization process, start the positioning system. After the measurement data obtained by each sensor in the positioning system for the target vehicle is input stably, use the roll angle, pitch angle, and heading angle obtained by the satellite positioning module and IMU to initialize the attitude parameters of the Kalman filter, use the three-dimensional position information obtained by the satellite positioning module and the altitude information obtained by the barometer to initialize the position parameters of the Kalman filter, and use the three-dimensional velocity information obtained by the satellite positioning module and the velocity information obtained by the vehicle odometer to initialize the velocity parameters of the Kalman filter.

[0068] S103: If it is determined that the target vehicle is in a moving state, according to the three-dimensional state observation quantity, determine the three-dimensional state of the target vehicle through the initialized Kalman filter.

[0069] Since the Kalman filter predicts the motion state of the vehicle at the next moment based on the motion state of the vehicle at the previous moment, in practical applications, it is necessary to first use the velocity information obtained by the vehicle odometer to determine whether the target vehicle is in a moving state, so as to use the Kalman filter combined with multiple sensors to achieve three-dimensional positioning of the target vehicle.

[0070] If it is determined that the target vehicle is in a moving state, then according to the three-dimensional state observation quantity obtained in S101 above, the Kalman filter can be used to perform an optimal estimate on the target vehicle through an equation to determine the three-dimensional state of the target vehicle. Among them, the three-dimensional state includes three-dimensional attitude, three-dimensional position, and three-dimensional velocity.

[0071] Considering that the error covariance matrix is involved in the calculation process, directly calculating the motion state of the target vehicle in three-dimensional space will result in large calculation errors. Therefore, in the actual calculation process, the plane and height calculations are separated, that is, the corresponding rows and columns of the error covariance matrix are calculated separately to eliminate the influence on other rows and columns, so that the calculations of height and plane using the Kalman filter do not interfere with each other, improving the calculation accuracy, and thus improving the accuracy of the three-dimensional positioning of the vehicle.

[0072] In the actual application process, the satellite positioning module may be blocked. In this scenario, the measurement data output by the satellite positioning module has low accuracy, or even cannot output measurement data in real time. Therefore, if it is detected that the blocking time of the satellite positioning module exceeds the time threshold, the data within the preset time after the satellite positioning module is unblocked is excluded. Among them, the time threshold and the preset time can be preset according to the actual scenario and are not limited here. In this way, the positioning accuracy of using the satellite positioning module for vehicle three-dimensional positioning is improved, and the reliability and stability of the entire system are improved.

[0073] Based on the above S101, the relative height position information of the target vehicle can be obtained by calibrating the measurement data output by the barometer based on a benchmark. In actual applications, the validity of the relative height information can also be verified to further improve the positioning accuracy of the vehicle in the height direction.

[0074] Specifically, the relative height position information is subjected to a validity test to obtain a first test result. If the first test result indicates that the relative height position information is valid, it means that the relative height position information can be used as the height position observation value of the target vehicle. Among them, the validity test can be set according to the actual scenario and is not limited here.

[0075] In actual applications, if the height position information obtained by the satellite positioning module meets the availability conditions, and the relative height position information obtained by the barometer does not meet the availability conditions, the floating-point solution height position information obtained by the satellite positioning module is subjected to a validity test to obtain a second test result. If the second test result indicates that the floating-point solution is valid, the floating-point solution height position information is used as the height position observation value of the target vehicle. That is to say, when it is determined that only the height position information output by the satellite positioning module is available, it is necessary to perform a validity test on its floating-point solution to determine that it is valid before it can be used as an observation value for observation update.

[0076] If the altitude position information obtained by the satellite positioning module meets the availability condition, and the relative altitude position information obtained by the barometer meets the availability condition, determine whether the altitude position information includes a fixed solution. If so, use the altitude position information of the fixed solution as the altitude position observation value of the target vehicle. If not, subtract the altitude position information obtained by the satellite positioning module and the relative altitude position information obtained by the barometer from the Kalman state recursive predicted altitude respectively, to obtain a first difference corresponding to the satellite positioning module and a second difference corresponding to the barometer. If the first difference is less than the second difference, use the altitude position information obtained by the satellite positioning module as the altitude position observation value of the target vehicle; if the first difference is not less than the second difference, use the altitude position information obtained by the satellite positioning module as the altitude position observation value of the target vehicle.

[0077] That is to say, when it is determined that the altitude information of both the satellite positioning module and the barometer is available, if the satellite positioning module has a fixed solution, use it as the observation value, otherwise subtract the altitude position information output by the satellite positioning module and the barometer from the Kalman state recursive predicted altitude respectively, and then compare the two differences to select the altitude value with the smallest height difference as the observation value of the filter.

[0078] The above-mentioned validity check of the measurement data output by the satellite positioning module and the barometer, and the separate calculation of altitude and plane during the solution process improve the reliability of the positioning system and also improve the positioning accuracy.

[0079] When performing plane position solution, the three-dimensional position information output by the satellite positioning module that meets the validity check condition can be used as the three-dimensional position observation value for observation update. In terms of speed solution, if it is determined that the vehicle speed in the altitude direction is available, perform Kalman filter altitude direction speed update. When it is determined that the vehicle is stationary, the Kalman filter observation update stops.

[0080] The vehicle three-dimensional positioning method provided by the above embodiments filters the sensor output information, improving the data accuracy of the vehicle three-dimensional state observation quantity and further enhancing the accuracy of calculating the vehicle three-dimensional state based on the three-dimensional state observation quantity; it uses the vehicle speed to correct the IMU data in real time to calculate the vehicle's three-axis gravitational acceleration components, which can accurately reflect the vehicle's acceleration and deceleration conditions and avoid the errors caused by directly using the IMU data; it calculates the pitch angle and roll angle using the vehicle's three-axis gravitational acceleration components and performs Kalman filtering for multi-sensor information fusion to finally output the vehicle attitude, improving the accuracy of attitude estimation; it decomposes the three-dimensional positioning solution into a plane solution and a height solution, enabling the height and plane solutions to not interfere with each other and reducing the errors existing in the solution process; it uses the speed information output by the vehicle odometer in real time to judge the vehicle's motion state and stops updating the state when the vehicle is stationary, making the vehicle state estimation more in line with reality; by designing a reasonable positioning system initialization step, the positioning result is more accurate; it uses a fusion algorithm for data processing, improving the accuracy of the vehicle three-dimensional positioning solution and enabling the vehicle to have good positioning accuracy in different driving scenarios.

[0081] Regarding the vehicle three-dimensional positioning method provided by the above embodiments, the embodiments of the present application further provide a vehicle three-dimensional positioning system. Refer to Figure 2 , Figure 2 which is a schematic structural diagram of a vehicle three-dimensional positioning system provided by the embodiments of the present application. As Figure 2 shown, the vehicle three-dimensional positioning system 200 includes a vehicle odometer 201, an inertial measurement unit IMU 202, a barometer 203, a satellite positioning module 204, and a Kalman filter 205; the vehicle three-dimensional positioning system 200 is used to execute the vehicle three-dimensional positioning method provided by the above embodiments.

[0082] The vehicle three-dimensional positioning system provided by the above embodiments obtains the three-dimensional state observation quantity of the target vehicle through the target vehicle positioning system. If it is determined that the three-dimensional state observation quantity meets the stability condition, it indicates that the measurement data output by the multiple sensors included in the positioning system tend to be in a stable state. Therefore, the Kalman filter in the positioning system can be initialized based on the three-dimensional state observation quantity, providing reliable measurement data for the Kalman filter to solve the three-dimensional state of the target vehicle. When the target vehicle is in a moving state, it indicates that the three-dimensional state of the target vehicle has changed. Therefore, based on the three-dimensional state observation quantity, the three-dimensional state of the target vehicle is determined through the initialized Kalman filter. Since the positioning system includes a vehicle odometer, an inertial measurement unit, a barometer, and a satellite positioning module, the Kalman filter performs fusion calculation on the measurement data obtained by these multiple sensors, improving the reliability of the positioning system and also enhancing the accuracy of three-dimensional positioning of the vehicle. This enables the vehicle to have good positioning accuracy in scenarios such as navigation that require height position information, thereby improving the user driving experience.

[0083] Those of ordinary skill in the art can understand that all or part of the steps of implementing the above method embodiments can be completed by hardware related to program instructions. The foregoing program can be stored in a computer-readable storage medium. When the program is executed, it performs the steps including the above method embodiments; and the foregoing storage medium can be at least one of the following media: read-only memory (ROM), RAM, magnetic disk, or optical disc, etc., which can store program codes.

[0084] It should be noted that the embodiments in this specification are all described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the device and system embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiments. The device and system embodiments described above are only illustrative. The units described as separate components may or may not be physically separated. The components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art can understand and implement it without creative efforts.

[0085] As described above, this is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed in the present application should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A three-dimensional vehicle positioning method, characterized in that, the method includes: obtaining three-dimensional state observation quantities of a target vehicle through a positioning system, where the three-dimensional state observation quantities include three-dimensional velocity observation quantities; the positioning system includes a vehicle odometer, an inertial measurement unit (IMU), a barometer, and a satellite positioning module; if it is determined that the three-dimensional state observation quantities meet the stability condition, initializing a Kalman filter in the positioning system according to the three-dimensional state observation quantities; if it is determined that the target vehicle is in a moving state, determining the three-dimensional state of the target vehicle through the initialized Kalman filter according to the three-dimensional state observation quantities; the obtaining of the three-dimensional state observation quantities of the target vehicle through the positioning system includes: obtaining three-dimensional velocity information of the target vehicle through the satellite positioning module, and obtaining velocity information of the target vehicle through the vehicle odometer; if the ground speed of the satellite positioning module is greater than a first threshold and the speed of the target vehicle in the height direction obtained through the satellite positioning module is less than a second threshold, using the three-dimensional velocity information corresponding to the satellite positioning module and the velocity information corresponding to the vehicle odometer as the three-dimensional velocity observation quantities of the target vehicle.

2. The method according to claim 1, characterized in that, the three-dimensional state observation quantities include three-dimensional attitude observation quantities, the three-dimensional attitude observation quantities include heading angle, roll angle, and pitch angle, and the obtaining of the three-dimensional state observation quantities of the target vehicle through the positioning system includes: obtaining velocity information of the target vehicle through the vehicle odometer, obtaining specific force information of the target vehicle through an accelerometer in the IMU, and obtaining angular velocity information of the target vehicle through a gyroscope in the IMU; filtering the velocity information, the specific force information, and the angular velocity information; correcting the filtered specific force information according to the filtered velocity information and the filtered angular velocity information to obtain the three-axis gravitational acceleration of the target vehicle; calculating the roll angle and pitch angle of the target vehicle according to the three-axis gravitational acceleration; obtaining the dual-antenna heading angle of the target vehicle through the satellite positioning module.

3. The method according to claim 2, characterized in that, the correcting the filtered specific force information according to the filtered velocity information to obtain the three-axis gravitational acceleration of the target vehicle includes: determining the forward linear acceleration of the target vehicle according to the velocity information; determining the centripetal acceleration of the target vehicle according to the velocity information and the angular velocity of the target vehicle's heading axis; determining the forward gravitational acceleration of the target vehicle according to the forward linear acceleration and the forward specific force obtained through the accelerometer; determining the lateral gravitational acceleration of the target vehicle according to the centripetal acceleration and the lateral specific force obtained through the accelerometer.

4. The method according to claim 1, characterized in that, the three-dimensional state observation quantities include three-dimensional position observation quantities, and the obtaining of the three-dimensional position observation quantities of the target vehicle through the positioning system includes: The three-dimensional position information of the target vehicle is obtained by the satellite positioning module; wherein the fixed solution and the floating point solution included in the three-dimensional position information are used as the three-dimensional position observation amount; The relative height position information of the target vehicle is obtained through the barometer; the relative height position information is used as the three-dimensional position observation quantity and is determined based on the fixed solution as a reference.

5. The method according to claim 4, It is characterized in that The method further comprises: Performing a validity check on the relative height position information to obtain a first check result; If the first inspection result indicates that the relative height position information is valid, the relative height position information is used as the height position observation value of the target vehicle.

6. The method according to claim 4, It is characterized in that The method further comprises: If the altitude position information acquired by the satellite positioning module meets the availability condition, and the relative altitude position information acquired by the barometer does not meet the availability condition, performing a validity check on the floating-point solution altitude position information acquired by the satellite positioning module to obtain a second check result; If the second verification result indicates that the floating-point solution is valid, the height position information of the floating-point solution is used as the height position observation value of the target vehicle.

7. The method according to claim 4, It is characterized in that The method further comprises: If the altitude position information acquired by the satellite positioning module meets the availability condition, and the relative altitude position information acquired by the barometer meets the availability condition, determining whether the altitude position information includes a fixed solution; If yes, use the fixed solution height position information as the height position observation of the target vehicle; If not, subtract the altitude position information obtained by the satellite positioning module and the relative altitude position information obtained by the barometer from the Kalman state recursive predicted altitude to obtain a first difference value corresponding to the satellite positioning module and a second difference value corresponding to the barometer; If the first difference is less than the second difference, using the height position information acquired by the satellite positioning module as the height position observation value of the target vehicle; If the first difference is not less than the second difference, the relative height position information obtained by the barometer is used as the height position observation value of the target vehicle.

8. The method according to any one of claims 1 to 7, It is characterized in that The method further comprises: If the satellite positioning module is blocked for a period of time exceeding a time threshold, data within a preset time after the satellite positioning module is unblocked is discarded.

9. A vehicle three-dimensional positioning system, It is characterized in that The system includes a vehicle odometer, an inertial measurement unit IMU, a barometer, a satellite positioning module and a Kalman filter; the system is used to execute the vehicle three-dimensional positioning method described in claims 1-8.

Images