High-precision positioning method, device and storage medium based on multi-sensor fusion

Abstract

The present application relates to the field of artificial intelligence technology, and provides a high-precision positioning method, device and storage medium based on multi-sensor fusion. The positioning method includes: obtaining the personal position information of the driver and the position status data of the driving vehicle driven by the driver, the position status data includes inertial navigation data and satellite positioning data; determining the vehicle position information of the target vehicle based on the position status data, the target vehicle includes the driving vehicle and\or the vehicle within the set range of the driving vehicle; comprehensively processing the personnel position information and the vehicle position information to obtain high-precision driving position information. In the above technical solution, since the position status data of the driving vehicle includes inertial navigation data and satellite positioning data, the position information of the target vehicle can be determined in combination with the inertial navigation data, which can help reduce the impact of the low and unstable satellite positioning accuracy on the vehicle positioning results, and further help improve the accuracy of the determined driving position information.

Description

Technical Field

[0001] The present application relates to the field of artificial intelligence technology, and in particular to a high-precision positioning method, device and storage medium based on multi-sensor fusion. Background Art

[0002] The Global Positioning System (GPS) is one of the most common navigation solutions. The Global Positioning System (GPS) is used to provide vehicle location and speed information, so that the vehicle location and motion state can be estimated.

[0003] However, the accuracy of GPS positioning is relatively low. In open areas with strong GPS signals and no obstructions, such as farmland and open roads, the accuracy that GPS positioning can provide is usually several meters to more than ten meters. When the GPS signal is affected by factors such as obstruction, multipath effects and signal interference, the accuracy of GPS positioning will decrease, which will affect the accuracy of the final positioning result. Summary of the invention

[0004] In order to help improve the accuracy of positioning results, the present application provides a high-precision positioning method, device and storage medium based on multi-sensor fusion.

[0005] In the first aspect, the present application provides a high-precision positioning method based on multi-sensor fusion, which adopts the following technical solution:

[0006] A high-precision positioning method based on multi-sensor fusion, the method comprising:

[0007] Acquiring the personal location information of the driver and the location status data of the vehicle driven by the driver, wherein the location status data includes inertial navigation data and satellite positioning data;

[0008] Determining vehicle position information of a target vehicle based on the position status data, the target vehicle including a driving vehicle and / or a vehicle within a set range of the driving vehicle;

[0009] The personnel position information and the vehicle position information are fused to obtain high-precision driving position information.

[0010] By adopting the above technical solution, since the position status data of the driving vehicle includes inertial navigation data and satellite positioning data, the position information of the target vehicle can be determined in combination with the inertial navigation data, which can help reduce the impact of the low accuracy and instability of satellite positioning on the vehicle positioning results, and further help improve the accuracy of the determined driving position information.

[0011] Optionally, the vehicle position information includes a driving vehicle position corresponding to the driving vehicle, and determining the vehicle position information of the target vehicle based on the position state data includes:

[0012] Determining whether to use the satellite positioning data for positioning;

[0013] When it is determined to use the satellite positioning data for positioning, the previous prediction uncertainty is calculated by using the Kalman gain equation to obtain the Kalman gain, and the previous prediction uncertainty is the prediction uncertainty determined last time;

[0014] Determining a current estimated state based on the Kalman gain, the satellite positioning data, a previous predicted state, and a satellite error state update equation, wherein the current estimated state includes a driving vehicle position, the satellite error state update equation is preset, and the previous predicted state is predetermined in combination with the last acquired inertial navigation data;

[0015] The estimation uncertainty is determined based on the Kalman gain, the previous prediction uncertainty and an error state covariance update equation, the error state covariance update equation is preset, and the previous uncertainty is determined in combination with the estimation uncertainty determined last time.

[0016] By adopting the above technical solution, before combining satellite positioning data for positioning, it is necessary to first determine whether to use satellite positioning data for positioning, and only combine satellite positioning data for positioning when the satellite positioning data has a certain reliability. This can help avoid the influence of satellite positioning data with large errors on the final positioning results, and thus help improve the accuracy of the positioning results.

[0017] Optionally, the position status data further includes UWB positioning data, and after determining whether to use the satellite positioning data for positioning, the method further includes:

[0018] When it is determined that the satellite positioning data is not used for positioning, the previous prediction uncertainty is calculated by using a Kalman gain equation to obtain a Kalman gain;

[0019] Determining the current estimated state based on the Kalman gain, the UWB positioning data, the previous predicted state and a UWB error state update equation, wherein the UWB error state update equation is preset;

[0020] An estimation uncertainty is determined based on the Kalman gain, the previous prediction uncertainty, and an error state covariance update equation.

[0021] By adopting the above technical solution, when it is determined that satellite positioning is not used, the Kalman gain, the UWB positioning data collected this time and the previous predicted state can be combined to perform error update to obtain the estimated position of the driving vehicle, which can help improve the accuracy of the final positioning result.

[0022] Optionally, before calculating the previous prediction uncertainty by using the Kalman gain equation to obtain the Kalman gain, the method further includes:

[0023] Inputting the position indicated by the inertial navigation data and the position indicated by the satellite positioning data into a preset satellite position error observation equation to obtain a satellite position observation error;

[0024] Inputting the speed indicated by the inertial navigation data and the speed indicated by the satellite positioning data into a preset satellite speed error observation equation to obtain a satellite speed observation error;

[0025] The measurement uncertainty in the Kalman gain equation is adjusted based on the satellite position observation error and the satellite velocity observation error to calculate the Kalman gain using the adjusted Kalman gain equation.

[0026] By adopting the above technical scheme, the measurement uncertainty in the Kalman gain can be adjusted in combination with the satellite position observation error and the satellite velocity observation error obtained by real-time observation, which can help to adjust the Kalman gain in combination with the actual situation of the satellite positioning data, and further help to improve the accuracy of the current estimated state of the driving vehicle estimated in combination with the Kalman gain.

[0027] Optionally, after determining the current estimated state, the method further includes:

[0028] The predicted state is determined based on the inertial navigation data, the current estimated state and an error state extrapolation equation, wherein the error state extrapolation equation is obtained by modeling the inertial navigation system of the driving vehicle, and the inertial navigation data is collected by the inertial navigation system; the predicted uncertainty is determined based on the estimated uncertainty and an error state covariance extrapolation equation, and the error covariance update equation is preset.

[0029] By adopting the above technical solution, the error state can be extrapolated by combining the inertial navigation data and the current estimated state to obtain the predicted state, which can help improve the accuracy of the determined predicted state, and further help improve the accuracy of the estimated position calculated next time.

[0030] Optionally, the error state extrapolation equation is constructed by combining an attitude error model, a velocity error model, a position error model and a sensor error model of the inertial navigation system.

[0031] By adopting the above technical solution, the influence of errors in different dimensions on the state prediction results can be taken into account in the process of error extrapolation through the error extrapolation equation, which can help improve the accuracy of the state prediction results.

[0032] Optionally, the error state extrapolation equation is constructed based on a system error vector of the inertial navigation system, and the system error vector is expressed by the following formula:

[0033] δx=[δr n δv n φ b g b a s g s a ]

[0034] Where δx represents the system error vector, δr n is the position error vector of the inertial navigation system, δv n is the velocity error vector of the inertial navigation system, φ is the attitude error vector of the inertial navigation system, b g is the zero bias vector of the three-axis gyroscope in the inertial navigation system, b a is the zero bias vector of the three-axis accelerometer in the inertial navigation system, s g is the scale factor error vector of the three-axis gyroscope, s a is the scale factor error vector of the three-axis accelerometer.

[0035] Optionally, the error state extrapolation equation is expressed by the following formula:

[0036] δx k =Φ k,k-1 δx k-1 +ω k-1

[0037] in,

[0038]

[0039]

[0040] δx k is the predicted state, Φ k,k-1 is the state transfer matrix, δx k-1 is the current estimated state, ω k-1 is the linear transformation of the system Gaussian white noise ω(t), F(t) is the system matrix, and G(t) is the input equation.

[0041] Optionally, the ω k-1 Satisfy k ~N(0,Q k ), the Qk is the system state noise covariance matrix, the Q k Calculated by the following formula:

[0042]

[0043] Among them, q(t) is a constant matrix, which is related to the sensor error model.

[0044] By adopting the above technical solution, the system state noise covariance matrix can be determined in combination with the sensor error model, which can help improve the accuracy of the final prediction result.

[0045] Optionally, the method further includes:

[0046] In the case where the personnel location information is not obtained, the driving location information is determined by combining the third-party positioning information provided by the third-party cloud platform and the vehicle location information.

[0047] By adopting the above technical solution, when obtaining personnel location information, the driving location information can be determined by combining the positioning information provided by the third-party cloud platform and the vehicle location, which can help ensure the accuracy of the driving location information.

[0048] In a second aspect, the present application also provides an electronic device, which adopts the following technical solution:

[0049] An electronic device, comprising:

[0050] at least one processor;

[0051] Memory;

[0052] At least one application, wherein the at least one application is stored in a memory and configured to be executed by at least one processor, and the at least one application is configured to: execute any one of the high-precision positioning methods based on multi-sensor fusion provided in the first aspect.

[0053] In a third aspect, the present application also provides a computer-readable storage medium, which adopts the following technical solution:

[0054] A computer-readable storage medium having a computer program stored thereon, which, when executed in a computer, causes the computer to execute any one of the high-precision positioning methods based on multi-sensor fusion provided in the first aspect.

[0055] In summary, the present application includes at least one of the following beneficial technical effects:

[0056] 1. Since the position status data of the driving vehicle includes inertial navigation data and satellite positioning data, the position information of the target vehicle can be determined in combination with the inertial navigation data, which can help reduce the impact of low and unstable satellite positioning accuracy on the vehicle positioning results, and further help improve the accuracy of the determined vehicle position information.

[0057] 2. Before combining satellite positioning data for positioning, it is necessary to first determine whether to use satellite positioning data for positioning, and only when the satellite positioning data has a certain degree of reliability, can the satellite positioning data be combined for positioning. This can help avoid the influence of satellite positioning data with large errors on the final positioning results, and thus help improve the accuracy of the positioning results. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 It is a flowchart of a multi-sensor fusion positioning method provided in an embodiment of the present application;

[0059] Figure 2 It is a flow chart of a method for determining vehicle position information provided by an embodiment of the present application;

[0060] Figure 3 It is a flowchart of another method for determining vehicle position information provided by an embodiment of the present application;

[0061] Figure 4 It is a flowchart of an error state extrapolation method provided in an embodiment of the present application;

[0062] Figure 5 It is a flow chart of a Kalman gain equation adjustment method provided in an embodiment of the present application;

[0063] Figure 6 It is a structural schematic diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0064] In order to make the purpose, technical solutions and advantages of this application more clear, the following Figure 1-6 It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0065] The present application embodiment discloses a high-precision positioning method based on multi-sensor fusion. Figure 1 ,The high precision positioning method based on multi-sensor fusion includes the following steps:

[0066] Step 101, obtaining the personal location information of the driver and the location status data of the vehicle driven by the driver, wherein the location status data includes inertial navigation data and satellite positioning data.

[0067] The personnel location information is collected by a personnel terminal device with a positioning function carried by the personnel. Specifically, the personnel terminal device can be a mobile phone, a mobile work terminal, a smart watch, etc., and this embodiment does not limit the personnel location information collection device. In actual implementation, the personnel terminal can determine the personnel location information by at least one positioning method of satellite positioning, cellular base station positioning, or other positioning methods, and this embodiment does not limit this.

[0068] The vehicle inertial navigation data is collected by the inertial measurement unit (IMU) in the inertial navigation system (hereinafter referred to as the inertial navigation system) installed on the driving vehicle. Generally, the inertial measurement unit includes a three-axis accelerometer and a three-axis gyroscope, and accordingly, the inertial navigation data includes the data of the three-axis accelerometer and the data collected by the three-axis gyroscope. In actual implementation, the inertial measurement unit can also include components such as a magnetometer, which is not limited to this.

[0069] Satellite positioning data is collected by a satellite positioning terminal installed on the driving vehicle. The satellite positioning terminal includes a positioning antenna and a receiving unit, which can be used to receive satellite positioning signals sent by satellites of at least one satellite positioning system such as the Beidou positioning system and the global positioning system (GPS). Accordingly, the satellite positioning data includes satellite positioning signals of at least one satellite positioning system. In one example, the satellite positioning signal includes position and speed.

[0070] Step 102: Determine vehicle position information of the target vehicle based on the position status data.

[0071] The target vehicle includes the driving vehicle and / or the vehicle within the set range of the driving vehicle. Specifically, the set range can be set according to actual needs, such as: within 3 meters, within 5 meters or within 10 meters, etc. This embodiment does not limit the size of the set range.

[0072] Optionally, in a case where the target vehicle includes a driving vehicle and a vehicle within a set range of the driving vehicle, the vehicle position information of the target vehicle is determined based on the position status data, including: determining the driving vehicle position information of the driving vehicle based on the position status data; determining the position information of the vehicle within the set range of the driving vehicle based on the driving vehicle position information and the relative position relationship between the vehicle within the set range of the driving vehicle and the driving vehicle.

[0073] In one example, the relative positions between vehicles are determined based on the Internet of Vehicles system. In one example, short-distance positioning modules (such as UWB positioning modules and Internet of Vehicles modules) are installed on different vehicles, and the relative positions of different vehicles can be determined by the short-distance positioning modules.

[0074] In other examples, the relative positions between vehicles may also be determined based on satellite positioning signals corresponding to different vehicles, which is not limited in this embodiment.

[0075] Step 103, comprehensively process the personnel position information and the vehicle position information to obtain high-precision driving position information.

[0076] In one example, the driving position information includes the position of the person and / or the position of the driving vehicle. In actual implementation, the driving position information may also include other information, such as the position of other persons in the driving vehicle and / or the position of other vehicles within a preset distance of the driving vehicle, which is not limited in this embodiment.

[0077] Under normal circumstances, the driver is usually in the driving vehicle, and the position of the person and the driving vehicle coincide. However, in some special circumstances, such as a traffic accident, the driver and the driving vehicle may be separated, so the position of the person and the driving vehicle need to be comprehensively processed.

[0078] In one example, the distance between the position indicated by the personnel position information and the driving vehicle position is less than or equal to a preset first distance threshold, which means that the driver is located on the driving vehicle. Since the accuracy of the driving vehicle position is higher than the accuracy of the driver's position information, the driving vehicle position can be determined as the personnel position, which can help improve the accuracy of the personnel position in the driving position information.

[0079] Furthermore, when the distance between the position indicated by the personnel position information and the driving vehicle position is greater than the first distance threshold and less than or equal to the second distance threshold (the second distance threshold is greater than the first distance threshold), it is further determined whether the difference between the change speed of the position indicated by the personnel position information and the change speed of the driving vehicle position information is less than or equal to a preset speed difference threshold; if so, it means that the driver is on the driving vehicle, and the driving vehicle position can also be determined as the personnel position.

[0080] In another example, the distance between the position indicated by the personnel location information and the driving vehicle position is greater than a preset second threshold, or the distance between the position indicated by the personnel location information and the driving vehicle position is greater than the first distance threshold and less than or equal to the second distance threshold, but the difference between the change speed of the position indicated by the personnel location information and the change speed of the driving vehicle location information is greater than the preset speed difference threshold. At this time, it means that the driver has left the driving vehicle. In this case, the relative position of the driver and the driving vehicle can be determined based on the personnel location information and satellite positioning data, and then the personnel position can be determined in combination with the driving vehicle position and the relative position of the driver and the driving vehicle. This can help improve the accuracy of the personnel position.

[0081] Optionally, the method provided in this embodiment also includes: when the personnel location information is not obtained, determining the driving location information by combining the third-party positioning information and the vehicle location information provided by the third-party cloud platform.

[0082] Among them, the third-party cloud platform refers to a third-party platform that provides positioning services, such as Baidu Maps, Amap, etc. It should be noted that there are differences between third-party positioning information and satellite positioning data. Specifically, the third-party positioning information is obtained by the third-party cloud platform after comprehensive processing of satellite positioning data and other data.

[0083] The situation where the personnel location information is not obtained includes, but is not limited to, a weak personnel location signal, a disconnection of the personnel terminal device, etc. This embodiment does not limit the situation where the personnel location information is not obtained.

[0084] In one example, the third-party positioning information is obtained and input by the driver. For example, the driver operates a third-party platform through a mobile phone to obtain the third-party positioning information, and then inputs the obtained third-party positioning information.

[0085] In actual implementation, the third row of positioning information can also be obtained automatically. For example, a third-party positioning software is installed on the driver's personal terminal or the vehicle-mounted terminal of the driving vehicle, and a corresponding data acquisition interface is configured. When there is a need for data acquisition, the third-party positioning information can be automatically obtained through the data acquisition interface, thereby reducing manual operations in the positioning process.

[0086] In the above technical solution, since the driving position information can be determined by combining the third-party positioning information and vehicle position information provided by the third-party cloud platform when the personnel position information is not obtained, this can help ensure the accuracy of the driving position information.

[0087] The implementation principle of a high-precision positioning method based on multi-sensor fusion in an embodiment of the present application is: obtaining the personal position information of the driver and the position status data of the driving vehicle driven by the driver, the position status data including inertial navigation data and satellite positioning data; determining the vehicle position information of the target vehicle based on the position status data, the target vehicle including the driving vehicle and\or the vehicle within the set range of the driving vehicle; comprehensively processing the personal position information and the vehicle position information to obtain high-precision driving position information. In the above technical scheme, since the position status data of the driving vehicle includes inertial navigation data and satellite positioning data, the position information of the target vehicle can be determined in combination with the inertial navigation data, which can help reduce the impact of the low and unstable satellite positioning accuracy on the vehicle positioning results, and further help improve the accuracy of the determined vehicle position information.

[0088] At the same time, since the personnel position information and vehicle position information can be comprehensively processed to obtain high-precision driving position information, this can facilitate the coordinated positioning between the driver and the driving vehicle, which can help reduce the error in the positioning process based on the relationship between the driver and the driving vehicle, and thus can help to further improve the positioning accuracy.

[0089] In some embodiments, the vehicle position information includes the driving vehicle position corresponding to the driving vehicle, referring to Figure 2 , step 201, determining the vehicle position information of the target vehicle based on the position status data, comprising the following steps:

[0090] Step 201, determining whether to use satellite positioning data for positioning.

[0091] During the research process, it was found that in some usage scenarios, the satellite positioning signal will be interfered with, resulting in a decrease in satellite positioning accuracy. At this time, combining satellite positioning data for positioning will affect the accuracy of the positioning results. Based on this, in this application, before combining satellite positioning data for positioning, it is necessary to first determine whether to use satellite positioning data for positioning.

[0092] In one example, determining whether to use satellite positioning data for positioning includes: determining whether the satellite positioning data exceeds a preset error threshold; if not, determining to use satellite positioning data for positioning and executing step 202; if so, it can be directly determined not to use satellite positioning data for positioning, or it can also be determined in combination with the judgment results of other dimensions whether to use satellite data for positioning.

[0093] In one instance, the error of the satellite positioning data is determined based on the delay of the satellite positioning signal. At this time, the preset error threshold is the preset maximum delay. Accordingly, determining whether the satellite positioning data exceeds the preset error threshold is to determine whether the delay corresponding to the satellite positioning data exceeds the preset maximum delay.

[0094] In another example, the location status data also includes ultra-wideband (UWB) positioning data. Since UWB positioning technology has the advantages of low power consumption, insensitivity to channel fading, strong anti-interference ability, no interference to other devices in the same environment, and strong penetration, it can help improve positioning accuracy.

[0095] Correspondingly, determining whether to use satellite positioning data for positioning includes: determining whether the satellite positioning data falls within the coverage of the UWB signal; if so, determining not to use satellite positioning data for positioning; if not, determining whether to use satellite positioning data for positioning can be combined with the judgment results of other dimensions (for example: the judgment result of whether the satellite positioning data exceeds a preset error threshold) or it can be directly determined to use satellite positioning data for positioning, and step 202 is executed.

[0096] Step 202: When it is determined that the satellite positioning data is used for positioning, the previous prediction uncertainty is calculated by using the Kalman gain equation to obtain the Kalman gain.

[0097] Among them, the previous unpredicted certainty is the prediction uncertainty determined last time.

[0098] Specifically, the Kalman gain equation is used to calculate the Kalman gain by combining the uncertainty of the previous prediction and the measurement uncertainty.

[0099] In one example, the measurement uncertainty is preset. In one example, in the Kalman gain equation corresponding to the satellite positioning data, the measurement uncertainty is set based on the error of the satellite positioning.

[0100] In one example, the Kalman gain equation includes sensor parameters for simulating sensor intrinsic parameters. In this case, the sensor parameters in the Kalman gain equation used in step 202 are used to simulate satellite positioning intrinsic parameters, which can help improve the accuracy of the calculated Kalman gain.

[0101] Step 203, determining a current estimated state based on the Kalman gain, the satellite positioning data, the previous predicted state and the satellite error state update equation, wherein the current estimated state includes the driving vehicle position.

[0102] Among them, the previous predicted state is determined in combination with the inertial navigation data obtained last time.

[0103] The satellite error state update equations are preset.

[0104] Specifically, the satellite error state update equation is used to estimate the current estimated state based on the Kalman gain, satellite positioning data and the previous predicted state.

[0105] In one example, the satellite error state update equation includes sensor parameters for simulating satellite positioning intrinsic parameters, which can help to accurately determine the current estimated state.

[0106] In actual implementation, the current estimated state may also include other data, such as the moving speed and posture of the driving vehicle, which may help to estimate the state of the driving vehicle later.

[0107] Step 204 , determining the estimation uncertainty based on the Kalman gain, the previous prediction uncertainty and the error state covariance update equation.

[0108] Among them, the error state covariance update equation is preset, and the previous uncertainty is determined in combination with the estimation uncertainty determined last time.

[0109] Specifically, the error covariance update equation is used to determine the uncertainty of the current estimated state based on the Kalman gain and the previous prediction uncertainty.

[0110] In one example, the error covariance update equation includes sensor parameters used to simulate satellite positioning intrinsics, which can help accurately determine the estimation uncertainty.

[0111] In the above technical solution, since the error can be updated by combining the Kalman gain, the satellite positioning data collected this time and the previous predicted state when using satellite positioning, the estimated position of the driving vehicle can be obtained, and the previous predicted state is determined in combination with the inertial navigation data obtained last time. In this way, the inertial navigation data can be integrated to determine the actual position of the driving vehicle while performing noise reduction through Kalman filtering, which can help improve the accuracy of the final positioning result.

[0112] Optionally, when it is determined that satellite positioning data is not used for positioning, the current estimated state of the driving vehicle can be determined in combination with other positioning signals, or the current estimated state of the driving vehicle can be determined directly based on inertial navigation data. This is not limited to the present embodiment. This can help avoid the impact of large errors in satellite positioning data on the current estimated state, which can help improve the accuracy of the current estimated state.

[0113] In one example, the previous predicted state is predetermined in combination with the inertial navigation data acquired last time and the estimated state determined last time. Accordingly, the estimated state determined last time may be determined in combination with the satellite positioning data, or may be determined without combining with the satellite positioning data.

[0114] In the above implementation, before combining satellite positioning data to position the driving vehicle, it is necessary to first determine whether to use satellite positioning data for positioning, and only when the satellite positioning data has a certain reliability, combine the satellite positioning data for positioning. This can help avoid the influence of satellite positioning data with large errors on the final positioning results, and thus help improve the accuracy of the positioning results.

[0115] In addition, when it is determined to use satellite positioning, the Kalman gain, the satellite positioning data collected this time and the previous predicted state can be combined to perform error update to obtain the estimated position of the driving vehicle, and the previous predicted state is determined in combination with the inertial navigation data obtained last time. In this way, the satellite positioning data can be denoised by Kalman filtering while the inertial navigation data can be integrated to determine the actual position of the driving vehicle, which can help improve the accuracy of the final positioning result.

[0116] In addition, since the uncertainty can be estimated based on the Kalman gain and the previous prediction uncertainty, and the previous prediction state is determined in combination with the estimated uncertainty determined last time, the estimated uncertainty can be updated according to actual conditions, thereby improving the accuracy of the prediction uncertainty determined based on the estimated uncertainty, which in turn can help improve the accuracy of the Kalman gain determined next time.

[0117] In order to help further improve the accuracy of the positioning results, in some embodiments, reference Figure 3 , the above step 201 further includes the following steps:

[0118] Step 301, when it is determined that the satellite positioning data is not used for positioning, the previous prediction uncertainty is calculated by using the Kalman gain equation to obtain the Kalman gain.

[0119] Among them, the previous unpredicted certainty is the prediction uncertainty determined last time.

[0120] Specifically, the Kalman gain equation is used to calculate the Kalman gain by combining the uncertainty of the previous prediction and the measurement uncertainty.

[0121] In one example, the measurement uncertainty is preset. In one example, in the Kalman gain equation corresponding to the UWB positioning data, the measurement uncertainty is set based on the error of the UWB positioning.

[0122] In one example, the Kalman gain equation includes sensor parameters for simulating sensor intrinsic parameters. In this case, the sensor parameters in the Kalman gain equation used in step 301 are used to simulate UWB measurement intrinsic parameters, which can help improve the accuracy of the calculated Kalman gain.

[0123] Step 302, determine the current estimated state based on the Kalman gain, the UWB positioning data, the previous predicted state and the UWB error state update equation.

[0124] Among them, the previous predicted state is determined in combination with the inertial navigation data obtained last time.

[0125] The UWB error state update equation is preset.

[0126] Specifically, the UWB error state update equation is used to estimate the current estimated state based on the Kalman gain, the UWB positioning data and the previous predicted state.

[0127] In one example, the UWB state update equation includes sensing parameters for simulating UWB measurement intrinsics, which can help accurately determine the current estimated state.

[0128] Step 303, determining the estimation uncertainty based on the Kalman gain, the previous prediction uncertainty and the error state covariance update equation.

[0129] Among them, the error covariance update equation is preset.

[0130] In one example, the error covariance update equation includes the sensing parameters used to simulate the UWB measurement intrinsics, which can help to accurately determine the estimation uncertainty.

[0131] It should be added that, although the accuracy of UWB positioning is better than that of satellite positioning, UWB positioning requires the establishment of a separate dedicated ground base station, and the coverage of a single base station is limited. As a result, in actual deployment, the coverage of the UWB positioning signal may be smaller than or even much smaller than the coverage of the satellite positioning signal. Therefore, UWB positioning is usually used to supplement satellite positioning, that is, UWB base stations are deployed in areas with low satellite positioning accuracy for UWB positioning. This can help control the deployment cost of the system while ensuring a certain positioning accuracy.

[0132] In actual implementation, since the accuracy of UWB positioning is higher than that of satellite positioning, when the driving vehicle is detected to move to the coverage area of ​​the UWB signal, it can be directly determined not to use the satellite positioning signal for positioning, but to use the UWB signal for positioning directly, so as to improve the positioning accuracy as much as possible. For example, when the satellite signal has a large impact in tunnels, the use of UWB positioning can very accurately obtain the location information of people and vehicles, and more accurately find the location information of the driver or vehicle, so that rescue can be obtained in time when an accident occurs.

[0133] In the above technical solution, since the error can be updated by combining the Kalman gain, the UWB positioning data collected this time and the previous predicted state when it is determined that satellite positioning is not used, the estimated position of the driving vehicle can be obtained, and the previous predicted state is determined in combination with the inertial navigation data obtained last time. In this way, the UWB positioning data can be denoised by Kalman filtering, and the inertial navigation data can be integrated to determine the actual position of the driving vehicle, which can help improve the accuracy of the final positioning result.

[0134] In some embodiments, reference Figure 4After the current estimation state is determined in step 203 or step 302, the following steps are also included:

[0135] Step 401, determining a predicted state based on inertial navigation data, a current estimated state and an error state extrapolation equation.

[0136] The error state extrapolation equation is obtained by modeling the inertial navigation system of the driving vehicle, and the inertial navigation data is collected by the inertial navigation system.

[0137] Specifically, the error state extrapolation equation is used to determine the predicted state based on the inertial navigation data and the current estimated state for use in determining the current estimated state next time.

[0138] Step 402, determining prediction uncertainty based on the estimation uncertainty and the error state covariance extrapolation equation.

[0139] Among them, the error state covariance extrapolation equation is preset.

[0140] Specifically, the error state covariance extrapolation equation is used to determine the prediction uncertainty based on the uncertainty for use in the next determination of the Kalman gain and the estimation uncertainty.

[0141] In the above technical solution, since the error state can be extrapolated by combining the inertial navigation data and the current estimated state to obtain the predicted state, this can help improve the accuracy of the determined predicted state, and further help improve the accuracy of the estimated position calculated next time.

[0142] In some implementations, the error state extrapolation equation in step 401 is constructed by combining an attitude error model, a velocity error model, a position error model, and a sensor error model of the inertial navigation system.

[0143] Among them, the attitude error model is used to analyze the attitude error of the inertial navigation system, the speed error model is used to analyze the speed error determined by the inertial navigation system, the position error model is used to analyze the position error determined by the inertial navigation system, and the sensor error model is used to analyze the error of the inertial measurement unit in the inertial navigation system.

[0144] Since the error extrapolation equation can be constructed from four-dimensional models of attitude error, velocity error, position error and sensor error, the impact of errors in different dimensions on the state prediction results can be taken into account in the process of error extrapolation through the error extrapolation equation, which can help improve the accuracy of the state prediction results.

[0145] Furthermore, the error state extrapolation equation is constructed based on the system error vector of the inertial navigation system, and the error vector is expressed by the following formula:

[0146] δx=[δr n δvn φ b g b a s g s a ]

[0147] Among them, δx represents the system error vector, δr n is the position error vector of the inertial navigation system, δv n is the velocity error vector of the inertial navigation system, φ is the attitude error vector of the inertial navigation system, b g is the zero bias vector of the three-axis gyroscope in the inertial navigation system, b a is the zero bias vector of the three-axis accelerometer in the inertial navigation system, s g is the three-axis gyroscope scale factor error vector, s a is the scale factor error vector of the three-axis accelerometer.

[0148] In an example, the position error vector, velocity error vector, attitude error vector, three-axis gyroscope bias vector, three-axis accelerometer bias vector, three-axis gyroscope scale factor error vector, and three-axis accelerometer scale factor error vector are all three-dimensional, so there are 21 dimensions in total.

[0149] In one example, the attitude error model is expressed in the form of an attitude error differential equation, as follows:

[0150]

[0151] Among them, φ np represents the attitude error vector, represents the derivative of the attitude error vector, It represents the projection vector of the local horizontal coordinate system (n system) relative to the angular velocity of the i system in the n system. is the direction cosine matrix representing the vehicle coordinate system (b system) relative to the real local horizontal coordinate system (n system), Represents the projection vector of the angular velocity of system b relative to system i on system b.

[0152] In the above formula, and

[0153]

[0154] Among them, δr N Represents the north position error in the local horizontal coordinate system, δr D represents the eastward position error in the local horizontal coordinate system, δr E represents the vertical position error in the local horizontal coordinate system, ω e is the angular velocity of the earth's rotation, h is the altitude of the vehicle, is the latitude of the vehicle, R M Represents the meridian curvature radius, R N represents the radius of curvature of the yoke, v N represents the northward velocity in the local horizontal coordinate system, v E It represents the vertical velocity in the local horizontal coordinate system.

[0155] Specifically, the derivation process of the attitude error differential equation is as follows:

[0156] First, define the attitude error. The attitude is used for the relative angle relationship between two coordinate systems. When there is a deviation in the attitude, it can be attributed to a certain coordinate system:

[0157]

[0158] The above formula represents a definition formula, in which, represents the attitude estimate of the vehicle coordinate system (b system) relative to the real local horizontal coordinate system (n system), Represents the direction cosine matrix of the vehicle coordinate system (b system) relative to the local horizontal coordinate system (n' system) with deviation, It represents the direction cosine matrix of the vehicle coordinate system (b system) relative to the p system, so the n' system is the p system.

[0159] The three Euler angles corresponding to the alignment of the real local horizontal coordinate system (n system) and the calculated local horizontal coordinate system (p system) are defined as the attitude error, which is defined as follows:

[0160]

[0161] Where I represents the 3×3 identity matrix, (φ np ×) represents the antisymmetric matrix composed of the vectors corresponding to the three Euler angles of the p system relative to the n system. In actual implementation, due to the high update frequency of the IMU, the time between adjacent epochs is very short, and the order of the rotation axes can be ignored. The vectors φ corresponding to the three Euler angles of the p system relative to the n system np It is called the attitude error vector, and the attitude error is generally small.

[0162] The target form of the inertial navigation attitude error differential equation is:

[0163]

[0164] The above formula indicates that the attitude error should be expressed as a first-order linear differential equation, δr n is the projection of the position error in the n system, δv n is the projection of the velocity error in the n system, φ is the attitude error, δf bThe projection of the accelerometer specific force error in the b system, is the projection of the gyroscope angular velocity error in frame b.

[0165] Known conditions:

[0166]

[0167]

[0168]

[0169]

[0170]

[0171]

[0172] Among them, equation (1) represents the transformation relationship between the direction cosine matrix of system b relative to system n' and the direction cosine matrix of system b relative to system n; equation (2) represents the relationship between the derivative of the direction cosine matrix of system b relative to system n and the direction cosine matrix itself and the projection vector of the angular velocity of system b relative to system n on system b; equation (3) represents the relationship between the projection vector of the angular velocity of system b relative to system n on system b and the projection vector of the angular velocity of system b relative to system i on system b and the projection vector of the angular velocity of system n relative to system i on system b; (4) represents the result after substituting equation (3) into equation (2); (5) represents the relationship between the projection vector of the angular velocity of system n relative to system i on system n and the projection vector of the angular velocity of system n relative to system i on system b; equation (6) represents the result derived from equations 4 and 5.

[0173] Derivative the equation (1) containing the attitude error, perturb the error in equation (6), and make their results equal to each other, then we can get the attitude error differential equation of the inertial navigation. The specific steps are as follows:

[0174] The first step is to derive formula (1):

[0175]

[0176] The second step is to perform error perturbation on Equation 6:

[0177]

[0178] The derivation of the above formula uses the following properties:

[0179]

[0180]

[0181]

[0182]

[0183]

[0184] Among them, formula (a) means that the perturbation value of the projection vector of the angular velocity of system b relative to system i on system b is equal to its true value plus its error term; formula (b) means that the perturbation value of the projection vector of the angular velocity of system n relative to system i on system n is equal to its true value plus its error term; formula (c) has the same meaning as formula (1); the left sides of the equal signs of formulas (d) and (e) contain two small quantities, so their product is approximately equal to 0;

[0185] Therefore, formula (8) can be simplified as:

[0186]

[0187] In the third step, equation (7) and equation (9) are equal:

[0188]

[0189] Arranged:

[0190]

[0191] Multiply both sides of the above formula by The transpose of is a unit orthogonal matrix, so the above formula is further simplified to:

[0192]

[0193] Further sorting:

[0194]

[0195] Because there are: Therefore, formula (10) can be further simplified as follows:

[0196]

[0197] because Therefore, formula (11) can be simplified as:

[0198]

[0199] Formula (12) can be written in vector form as follows:

[0200]

[0201] That is, the above attitude error differential equation is obtained.

[0202] In one embodiment, the velocity error model is expressed in the form of a velocity error differential equation as follows:

[0203]

[0204] Among them, v n represents the velocity vector in the local horizontal coordinate system, δv n represents the velocity error vector in the local horizontal coordinate system, represents the derivative of the velocity error vector, is the direction cosine matrix representing the vehicle coordinate system (b system) relative to the real local horizontal coordinate system (n system), f b represents the projection of the specific force measured by the inertial navigation system in the b system, f n It represents the projection of the specific force measured by inertial navigation in the n-frame, φ np represents the attitude error vector, It represents the projection of the angular velocity of the e system relative to the i system (the angular velocity of the earth's rotation) in the n system. represents the projection of the angular velocity of the n system relative to the e system in the n system, v n represents the velocity in the local horizontal coordinate system, represents the local gravity error.

[0205] Specifically, the derivation process of the velocity error differential equation is as follows:

[0206] The velocity error differential equation modeling of the inertial navigation has the target form:

[0207]

[0208] The above formula indicates that the velocity error should be expressed as a first-order linear differential equation, and the known conditions are:

[0209]

[0210] in, represents the projection of the first-order derivative of the velocity vector in the n-frame, f b It represents the projection of the specific force measured by the inertial navigation system in the b system. The first term in the brackets is It means twice the projection of the angular velocity of the e system relative to the i system (the angular velocity of the earth's rotation) in the n system. The second term in the brackets represents the projection of the angular velocity of the n system relative to the e system in the n system, v n It represents the velocity in the local horizontal coordinate system;

[0211] g p The expansion of is:

[0212]

[0213] Where g represents the gravity vector, ω ie represents the angular velocity vector of the e system relative to the i system, and r represents the position vector of the vehicle, so:

[0214]

[0215] The derivation idea is to perform perturbation analysis on equation (14), expand it term by term and obtain the first-order term. The derivation process is as follows:

[0216]

[0217]

[0218] Among them, the superscript “^” represents the disturbance of the corresponding physical quantity, and the symbol “δ” represents the error of the corresponding physical quantity. Except for the attitude disturbance, the disturbance value of other physical quantities is equal to the true value plus its error. The following properties are used in the derivation of formula (17) above:

[0219]

[0220]

[0221]

[0222]

[0223]

[0224] Ignoring the second-order small quantity in equation (17), we have:

[0225]

[0226] Combining (14), we have:

[0227]

[0228] in, represents the angular velocity of the n system relative to the e system, represents the angular velocity error of the n system relative to the e system, v n represents the velocity vector in the local horizontal coordinate system, δv n represents the velocity error vector in the local horizontal coordinate system, represents the local gravity error, and further sorting gives:

[0229]

[0230] Formula (18) is the velocity error differential equation of the above-mentioned inertial navigation.

[0231] In one example, the position error model is represented by the velocity error differential equation established in the n-frame, as follows:

[0232]

[0233] in, is the latitude of the vehicle, h is the altitude of the vehicle, R M Represents the meridian curvature radius, R N represents the radius of curvature of the yoke, v N represents the northward velocity in the local horizontal coordinate system, v D represents the eastward velocity in the local horizontal coordinate system, v E represents the vertical velocity in the local horizontal coordinate system, δv N is the north velocity error, δv E is the eastward velocity error, δv D represents the vertical velocity error, Indicates the relationship between δv N Find derivatives; Indicates the relationship between δv E Find the derivative, Indicates the relationship between δv D Find the derivative.

[0234] Specifically, the derivation process of the position error differential equation is as follows:

[0235] The position error equation modeling of the inertial navigation system is in the form of the position error differential equation in the sense of longitude and latitude:

[0236]

[0237] In the above formula, is the latitude of the vehicle, λ is the longitude of the vehicle, h is the altitude of the vehicle, and each component on the left side of the equal sign is its own derivative; the meanings of the other components have been explained before and will not be repeated here;

[0238] The perturbation analysis of the latitude derivative is:

[0239]

[0240] The perturbation analysis of the derivative of longitude is:

[0241]

[0242] The perturbation analysis of the derivative of elevation is:

[0243]

[0244] When the position error differential equation of the inertial navigation is established in the n-system, the above position error differential equation is obtained.

[0245] In one example, the inertial measurement unit of the inertial navigation system includes an accelerometer and a gyroscope, and the error model of the accelerometer is expressed by the following formula:

[0246]

[0247]

[0248] in, Represents the actual specific force measurement output of the accelerometer, f b represents the theoretical output of the accelerometer, I is the unit vector, s a is the scale factor error vector of the accelerometer, b a is the accelerometer zero bias vector, ω a represents the white noise of the accelerometer measurements, δf b Represents the accelerometer measurement error.

[0249] Set the accelerometer zero bias b a and the accelerometer scale factor error S a Modeled as a first-order Gauss-Markov process, we have:

[0250]

[0251]

[0252] Among them, T ab represents the relative time of the accelerometer bias, T as represents the relative time of the accelerometer scale factor, ω ab represents the accelerometer zero bias driving white noise, ω as Represents the accelerometer scale factor driving the white noise.

[0253] The gyroscope error model is expressed by the following equation:

[0254]

[0255]

[0256] in, Represents the actual angular velocity measurement output of the gyroscope, is the theoretical angular velocity in system b, S g is the scale factor error vector of the gyroscope, b g is the gyro bias vector, ω g represents the white noise of the gyroscope measurements, Represents the gyroscope measurement error.

[0257] Set the gyro to zero bias b gand gyro scale factor error S g Modeled as a first-order Gauss-Markov process, we have:

[0258]

[0259]

[0260] Among them, T ab represents the relative time of the gyro bias, T as represents the relative time of the gyroscope scale factor, ω ab represents the gyroscope zero bias driving white noise, ω as Represents the gyroscope scale factor driving white noise.

[0261] In some implementations, the error state extrapolation equation in step 401 is expressed by the following equation:

[0262] δx k =Φ k,k-1 δx k-1 +ω k-1

[0263] in,

[0264]

[0265]

[0266] δx k is the predicted state, Φ k,k-1 is the state transfer matrix, δx k-1 is the current estimated state, ω k-1 is the linear transformation of the system Gaussian white noise ω(t), F(t) is the system matrix, and G(t) is the input equation.

[0267] In one example, the system state error differential equation modeling of the inertial navigation system is performed through the error vector of the inertial navigation system in the above example, the structure of the attitude error differential equation modeling of the inertial navigation system, the velocity error differential equation modeling of the inertial navigation system, the position error differential equation modeling of the inertial navigation system, and the sensor error modeling of the inertial navigation system, and the following is obtained:

[0268]

[0269] The above error state extrapolation equation can be obtained through formula (28), and the system matrix F(t) is derived through the structural derivation of the attitude error differential equation modeling of the inertial navigation system, the velocity error differential equation modeling of the inertial navigation system, the position error differential equation modeling of the inertial navigation system, and the sensor error modeling of the inertial navigation system.

[0270] Furthermore, k-1Satisfy k ~N(0,Q k ), Q k is the system state noise covariance matrix, Q k Calculated by the following formula:

[0271]

[0272] Wherein, q(t) is a constant matrix, which is related to the sensor error model. This can help improve the accuracy of the determined system noise, and further help improve the accuracy of the final prediction result.

[0273] Specifically, the calculation process of the above system state noise covariance matrix is ​​as follows:

[0274]

[0275]

[0276] In the above formula, q(t) is the power spectrum density matrix, which is a constant matrix and is related to the sensor error model parameters. T (t) In the time interval [t k-1 , t k ] When the change within is not large, the trapezoidal integral formula can be used to derive the following equation:

[0277]

[0278] This gives the above Q k The calculation method of .

[0279] In some embodiments, the satellite positioning signal includes a position. The high-precision positioning algorithm based on multi-sensor fusion provided in the embodiment of the present application further includes: calculating the position in the satellite positioning signal through a satellite position error observation equation to obtain a satellite position observation error.

[0280] Specifically, the satellite position error observation equation is expressed by the following formula (the satellite positioning is GPS positioning as an example for explanation below):

[0281] δz r =H r δx+δr G

[0282] In the above formula, H r Represents the position observation matrix of satellite positioning, the specific expression is:

[0283]

[0284] Among them, δz r is the satellite position observation error observation vector, δrG Indicates the GPS position error, is the direction cosine matrix representing the vehicle coordinate system (b system) relative to the real local horizontal coordinate system (n system), l b is the projection of the arm vector of the GPS phase center in the b system.

[0285] The specific derivation process of the satellite position error observation equation is as follows:

[0286] First, considering the arm effect, we have:

[0287]

[0288] In the above formula, r GPS Indicates the actual position measured by the GPS antenna, r IMU Indicates the actual position measured by the inertial navigation. The position here is expressed in latitude, longitude and altitude. b is the projection of the arm vector of the GPS phase center in the b system, The function of is to convert the local horizontal coordinates into coordinates in the form of latitude, longitude and height, which are as follows:

[0289]

[0290] Among them, R M Represents the meridian curvature radius, R N represents the radius of curvature of the y-axis circle; is the latitude of the vehicle; h is the altitude of the vehicle.

[0291] Taking the error into account, we have:

[0292]

[0293] Among them, variables with superscript ^ represent calculated values ​​with errors. Represents the projection of the position vector of the GPS phase center calculated by inertial navigation in the n-frame, Represents the projection of the position vector of the IMU center calculated by inertial navigation in the n system;

[0294]

[0295] Where I is a 3×3 identity matrix, (φ np ×) is the antisymmetric matrix composed of the vectors corresponding to the three Euler angles of the p system relative to the n system, δr n is the projection of the position error in the n system.

[0296] Substituting (30) into (29) yields:

[0297]

[0298] The position of the GPS antenna phase center obtained by GPS positioning is expressed as:

[0299]

[0300] in, Represents the GPS position observation value, Represents the actual position vector of GPS, δr G Indicates the GPS position error.

[0301] Correspondingly, the observation vector is expressed as the difference between the inertial navigation position and the GPS position observation, which is:

[0302]

[0303] Therefore, the GPS position observation equation can be written as follows:

[0304] δz r =H r δx+δr G

[0305] Among them, H r Represents the GPS position observation matrix, the specific expression is:

[0306]

[0307] Furthermore, when the positioning signal includes velocity, based on the same principle, the satellite velocity error observation equation can be derived. Specifically, the satellite velocity error observation equation is expressed by the following formula:

[0308] δz v =H v δx+δv G

[0309] Among them, δv G Indicates GPS velocity error, H v Represents the GPS position observation matrix, the specific expression is:

[0310]

[0311] in:

[0312]

[0313]

[0314] The symbol diag represents a diagonal matrix operation.

[0315] Optionally, the above step 201, determining whether to use satellite positioning data for positioning, includes: determining whether to use satellite positioning data for positioning in combination with the satellite position observation error and satellite velocity observation error corresponding to the satellite positioning data, wherein the satellite position observation error is obtained by the satellite position error observation equation provided in the above embodiment, and the satellite velocity observation error is obtained by the satellite velocity error observation equation provided in the above embodiment.

[0316] Further, in some embodiments, reference Figure 5 In step 103, the previous prediction uncertainty is calculated by the Kalman gain equation to obtain the Kalman gain, and the following steps are also included:

[0317] Step 501: input the position indicated by the inertial navigation data and the position indicated by the satellite positioning data into a preset satellite position error observation equation to obtain the satellite position observation error.

[0318] In one example, the satellite position error observation equation is the satellite position error observation equation provided in the above embodiment, and its specific implementation method is not repeated here in this embodiment.

[0319] In actual implementation, the satellite position error observation equation may also be set based on other methods, as long as the satellite position observation error can be calculated.

[0320] Step 502: Input the speed indicated by the inertial navigation data and the speed indicated by the satellite positioning data into a preset satellite speed error observation equation to obtain a satellite speed observation error.

[0321] In one example, the satellite velocity error observation equation is the satellite velocity error observation equation provided in the above embodiment, and its specific implementation method is not repeated here in this embodiment.

[0322] Step 503: Adjust the measurement uncertainty in the Kalman gain equation based on the satellite position observation error and the satellite velocity observation error, so as to calculate the Kalman gain using the adjusted Kalman gain equation.

[0323] Among them, the corresponding relationship between the satellite position observation error and the satellite velocity observation error and the measurement uncertainty is preset. Specifically, the larger the satellite position observation error and the satellite velocity observation error, the larger the measurement uncertainty. Further, corresponding weights can be set for the satellite position observation error and the satellite velocity observation error to adjust the influence of the satellite position observation error and the satellite velocity observation error on the measurement uncertainty.

[0324] In the above technical scheme, since the measurement uncertainty in the Kalman gain can be adjusted in combination with the satellite position observation error and the satellite velocity observation error obtained by real-time observation, it can help to adjust the Kalman gain in combination with the actual situation of the satellite positioning data, and further help to improve the accuracy of the current estimated state of the driving vehicle estimated in combination with the Kalman gain.

[0325] In some embodiments, the UWB positioning signal includes a position. The high-precision positioning algorithm based on multi-sensor fusion provided in the embodiment of the present application further includes: calculating the position in the satellite positioning signal through the UWB position error observation equation to obtain the UWB position observation error.

[0326] Specifically, the UWB position error observation equation is expressed by the following formula:

[0327]

[0328] In the above formula, δr UWB is the UWB position observation error, Indicates the position of the UWB tag solved by the inertial navigation system, Indicates the error-bearing position calculated by the UWB tag itself. The function is to convert the local horizontal coordinates into coordinates in the form of latitude, longitude and height.

[0329] In one example, the UWB tag itself resolves its position in the following steps:

[0330] S0, UWB tag is fixed at the center of the vehicle, UWB base station is fixed on the ground with known position (longitude, latitude and elevation), the number of base stations is set to 4, recorded as base station No. 1, base station No. 2, base station No. 3 and base station No. 4;

[0331] S1. Use binocular ranging principle to determine the distance of the manually set road signs, so as to determine the absolute position of the vehicle. The specific ranging method is:

[0332]

[0333]

[0334]

[0335]

[0336] In the above formula, T represents the flight time interval, T 14 It represents the time interval from when the tag sends a request to when it receives a feedback signal, T 23 It represents the time interval from the base station receiving the request to sending the feedback signal. The superscript represents the corresponding base station number.1 , T 2 , T 3 and T 4 They are the signal flight time from the UWB tag on the vehicle to the four base stations.

[0337] S2. Calculate the distance of the tag relative to each base station. The calculation formula is:

[0338] D 1 =cT 1

[0339] D 2 =cT 2

[0340] D 3 =cT 3

[0341] D 4 =cT 4

[0342] In the above formula, c is the speed of light, D 1 , D 2 , D 3 and D 4 They are the distances from the UWB tag on the vehicle to the four base stations.

[0343] S3. Calculate the coordinates of the tag on the vehicle relative to the base station. Suppose the coordinates of the first base station are (x1, y1, z1), the coordinates of the second base station are (x2, y2, z2), the coordinates of the third base station are (x3, y3, z3), and the coordinates of the fourth base station are (x4, y4, z4). The tag coordinates are rUWB = (x, y, z). Then there is the following relationship:

[0344]

[0345]

[0346]

[0347]

[0348] The coordinates of the tag relative to the base station can be obtained by using the least squares method, and then the coordinates of the base station can be combined to obtain the calculated position of the UWB tag itself, that is,

[0349] Optionally, based on the same principle, in the above step 301, the previous prediction uncertainty is calculated by the Kalman gain equation. Before obtaining the Kalman gain, the UWB position observation error can be obtained based on the above-mentioned UWB position error observation model, and the measurement uncertainty of the Kalman gain equation used in step 301 can be adjusted based on the UWB position observation error, which can help improve the accuracy of state estimation.

[0350] In some implementations, in the above step 201, when determining whether to use satellite positioning data for positioning, the UWB position observation error obtained based on the above UWB position error observation model may also be used for judgment, so that it is easy to choose between satellite positioning and UWB positioning.

[0351] The present application also provides an electronic device. Figure 6 As shown, Figure 6 The electronic device 600 shown includes: a processor 601 and a memory 603. The processor 601 and the memory 603 are connected, such as through a bus 602. Optionally, the electronic device 600 may also include a transceiver 604. It should be noted that in actual applications, the transceiver 604 is not limited to one, and the structure of the electronic device 600 does not constitute a limitation on the embodiments of the present application.

[0352] Processor 601 may be a CPU (Central Processing Unit), a general purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit) or other programmable logic devices, transistor logic devices, hardware components or any combination thereof. It may implement or execute various exemplary logic blocks, modules and circuits described in conjunction with the disclosure of this application. Processor 601 may also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, etc.

[0353] The bus 602 may include a path to transmit information between the above components. The bus 602 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus. The bus 602 may be divided into an address bus, a data bus, etc. For ease of representation, Figure 6 Only one thick line is used in the diagram, but this does not mean that there is only one bus or only one type of bus.

[0354] The memory 603 can be a ROM (Read Only Memory) or other types of static storage devices that can store static information and instructions, a RAM (Random Access Memory) or other types of dynamic storage devices that can store information and instructions, or an EEPROM (Electrically Erasable Programmable Read Only Memory), a disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store the desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited to these.

[0355] The memory 603 is used to store the application code for executing the solution of the present application, and the execution is controlled by the processor 601. The processor 601 is used to execute the application code stored in the memory 603 to implement the contents shown in the above method embodiment.

[0356] The electronic devices include, but are not limited to, mobile terminals such as mobile phones, notebook computers, PDAs (personal digital assistants), PADs (tablet computers), etc., and fixed terminals such as digital TVs, desktop computers, etc. It can also be a server terminal, etc. Figure 6 The electronic device shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0357] An embodiment of the present application also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed in a computer, the computer is caused to execute the high-precision positioning method based on multi-sensor fusion provided in the above embodiment.

[0358] It should be understood that although the steps in the flowchart of the accompanying drawings are shown in sequence according to the instructions of the arrows, these steps are not necessarily performed in the order indicated by the arrows. Unless otherwise clearly stated in this document, the execution of these steps is not strictly limited in order and can be performed in other orders.

[0359] The above are only some implementation methods of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A high-precision positioning method based on multi-sensor fusion, characterized in that: The method comprises: Acquiring the personal location information of the driver and the location status data of the vehicle driven by the driver, wherein the location status data includes inertial navigation data and satellite positioning data; Determine vehicle position information of a target vehicle based on the position status data, the target vehicle including the driving vehicle and / or vehicles within a set range of the driving vehicle; Fusing the personnel position information and the vehicle position information to obtain high-precision driving position information; The vehicle position information includes the driving vehicle position corresponding to the driving vehicle, and the vehicle position information of the target vehicle is determined based on the position state data, including: Determining whether to use the satellite positioning data for positioning; In the case of determining to use the satellite positioning data for positioning, calculating the previous prediction uncertainty through the Kalman gain equation to obtain the Kalman gain, wherein the previous prediction uncertainty is the prediction uncertainty determined last time, and the measurement uncertainty in the Kalman gain equation is set based on the error of the satellite positioning; Determining a current estimated state based on the Kalman gain, the satellite positioning data, a previous predicted state, and a satellite error state update equation, wherein the current estimated state includes a driving vehicle position, the satellite error state update equation is preset, the previous predicted state is predetermined in combination with the last acquired inertial navigation data, and the satellite error state update equation includes a sensor parameter for simulating a satellite positioning internal parameter; Determining an estimation uncertainty based on the Kalman gain, a previous uncertainty, and an error state covariance update equation, the error state covariance update equation being preset, the previous uncertainty being determined in conjunction with a last determined estimation uncertainty, the error state covariance update equation including sensor parameters for simulating satellite intrinsic parameters; Determining a predicted state based on the inertial navigation data, the current estimated state and an error state extrapolation equation, wherein the error state extrapolation equation is obtained by modeling an inertial navigation system of the driving vehicle, and the inertial navigation data is collected by the inertial navigation system; The prediction uncertainty is determined based on the estimation uncertainty and an error state covariance extrapolation equation, and the error state covariance extrapolation equation is preset.

2. The method according to claim 1, characterized in that The position status data also includes UWB positioning data. After determining whether to use the satellite positioning data for positioning, the method further includes: When it is determined that the satellite positioning data is not used for positioning, the previous prediction uncertainty is calculated by using a Kalman gain equation to obtain a Kalman gain; Determining the current estimated state based on the Kalman gain, the UWB positioning data, the previous predicted state and a UWB error state update equation, wherein the UWB error state update equation is preset; An estimation uncertainty is determined based on the Kalman gain, the previous uncertainty, and an error state covariance update equation.

3. The method according to claim 2, characterized in that Before calculating the previous prediction uncertainty by the Kalman gain equation to obtain the Kalman gain, the method further includes: Inputting the position indicated by the inertial navigation data and the position indicated by the satellite positioning data into a preset satellite position error observation equation to obtain a satellite position observation error; Inputting the speed indicated by the inertial navigation data and the speed indicated by the satellite positioning data into a preset satellite speed error observation equation to obtain a satellite speed observation error; The measurement uncertainty in the Kalman gain equation is adjusted based on the satellite position observation error and the satellite velocity observation error to calculate the Kalman gain using the adjusted Kalman gain equation.

4. The method according to claim 3, characterized in that The error state extrapolation equation is constructed by combining the attitude error model, velocity error model, position error model and sensor error model of the inertial navigation system.

5. The method according to claim 3 or 4, characterized in that: The error state extrapolation equation is constructed based on the system error vector of the inertial navigation system, and the system error vector is expressed by the following formula: δx=[δr n dv n f b g b a s g s a ] Where δx represents the system error vector, δr n is the position error vector of the inertial navigation system, δv n is the velocity error vector of the inertial navigation system, φ is the attitude error vector of the inertial navigation system, b g is the zero bias vector of the three-axis gyroscope in the inertial navigation system, b a is the zero bias vector of the three-axis accelerometer in the inertial navigation system, s g is the scale factor error vector of the three-axis gyroscope, s a is the scale factor error vector of the three-axis accelerometer.

6. The method according to claim 5, characterized in that The error state extrapolation equation is expressed by the following formula: δx k =Φ k,k-1 δx k-1 +oh k-1 in, , , δx k is the predicted state, Φ k,k-1 is the state transfer matrix, δx k-1 is the current estimated state, ω k-1 is the linear transformation of the system Gaussian white noise ω(t), F(t) is the system matrix, and G(t) is the input equation.

7. The method according to claim 6, characterized in that The ω k-1 Satisfy k ~N(0,Q k ), the Q k is the system state noise covariance matrix, the Q k Calculated by the following formula: , Among them, q(t) is a constant matrix, which is related to the sensor error model.

8. The method according to claim 1, characterized in that: The method further comprises: In the case where the personnel location information is not obtained, the driving location information is determined by combining the third-party positioning information provided by the third-party cloud platform and the vehicle location information.

9. An electronic device, characterized in that: The electronic device comprises: at least one processor; Memory; At least one application, wherein the at least one application is stored in a memory and configured to be executed by at least one processor, and the at least one application is configured to: execute the high-precision positioning method based on multi-sensor fusion as described in any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed in a computer, the computer is caused to execute the high-precision positioning method based on multi-sensor fusion as described in any one of claims 1 to 8.

Images