CN118191891A - Vehicle positioning method, device, electronic device and readable storage medium - Google Patents

Abstract

The present application relates to the field of positioning technology, and provides a vehicle positioning method, device, electronic device and readable storage medium. The method includes: obtaining the first measurement data and the second measurement data of the vehicle at the current moment; determining the first position information corresponding to the vehicle at the current moment according to the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, and determining the second position information corresponding to the vehicle at the current moment according to the position information of multiple ultra-wideband base stations at the current moment and the distance information corresponding to each ultra-wideband base station and the vehicle; based on the first position information of the vehicle at the current moment and the second position information of the vehicle at the current moment, predicting the position information of the vehicle at the current moment through a factor graph model, and obtaining the target position information of the vehicle at the current moment. Solve the problem of inaccurate positioning of other positioning technologies in satellite-denied environments in the prior art, and improve the positioning accuracy of vehicles in satellite-denied environments.

Description

Vehicle positioning method, device, electronic device and readable storage medium

Technical Field

The present application relates to the field of positioning technology, and in particular to a vehicle positioning method, device, electronic device and readable storage medium.

Background technique

At present, the Global Navigation Satellite System (GNSS) is widely used for vehicle positioning. However, in satellite-denied environments such as tunnels, underground garages, and buildings with severe shielding, the communication between the vehicle and the satellite is interrupted, resulting in the failure of the GNSS positioning function. In satellite-denied environments, commonly used positioning technologies include: Inertial Measurement Unit (IMU), Ultra Wideband Band (UWB), LiDAR, ultrasound, geomagnetism, and Bluetooth. The positioning information obtained by relying on IMU is not absolute position information, and the positioning error of IMU will diverge over time and cannot continuously provide reliable position information. For wireless sensor positioning methods represented by UWB, the non-line-of-sight error of UWB is large in complex positioning environments. In the case of GNSS signal failure, other positioning technologies have certain limitations and cannot accurately locate the vehicle position.

Summary of the invention

In view of this, the embodiments of the present application provide a vehicle positioning method, device, electronic device and readable storage medium to solve the problem of inaccurate positioning of other positioning technologies when the GNSS signal fails in the prior art.

According to a first aspect of an embodiment of the present application, a vehicle positioning method is provided, comprising: obtaining first measurement data and second measurement data of the vehicle at a current moment, the first measurement data being vehicle speed information measured by an inertial measurement unit on the vehicle, and the second measurement data being position information of a plurality of ultra-wideband base stations and respective distance information corresponding to each ultra-wideband base station and the vehicle; determining first position information corresponding to the vehicle at the current moment according to the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, and determining second position information corresponding to the vehicle at the current moment according to the position information of a plurality of ultra-wideband base stations at the current moment and respective distance information corresponding to each ultra-wideband base station and the vehicle; predicting the position information of the vehicle at the current moment based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment by using a factor graph model to obtain target position information of the vehicle at the current moment, the factor graph model being constructed based on historical vehicle speed information and historical position information of a plurality of ultra-wideband base stations and respective distance information corresponding to each ultra-wideband base station and historical vehicles.

According to a second aspect of an embodiment of the present application, a vehicle positioning device is provided, comprising: an acquisition module, configured to acquire first measurement data and second measurement data of the vehicle at a current moment, wherein the first measurement data is vehicle speed information measured by an inertial measurement unit on the vehicle, and the second measurement data is position information of multiple ultra-wideband base stations and respective distance information corresponding to each ultra-wideband base station and the vehicle; a calculation module, configured to determine the first position information corresponding to the vehicle at the current moment according to the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, and to determine the second position information corresponding to the vehicle at the current moment according to the position information of multiple ultra-wideband base stations at the current moment and respective distance information corresponding to each ultra-wideband base station and the vehicle; a prediction module, configured to predict the position information of the vehicle at the current moment through a factor graph model based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment, so as to obtain the target position information of the vehicle at the current moment, wherein the factor graph model is constructed based on historical vehicle speed information and historical position information of multiple ultra-wideband base stations and respective distance information corresponding to each ultra-wideband base station and historical vehicles.

According to a third aspect of an embodiment of the present application, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

According to a fourth aspect of an embodiment of the present application, a readable storage medium is provided, which stores a computer program, and when the computer program is executed by a processor, the steps of the above method are implemented.

Compared with the prior art, the embodiments of the present application have the following beneficial effects: by obtaining the first measurement data and the second measurement data of the vehicle at the current moment, the first measurement data is the vehicle speed information measured by the inertial measurement unit on the vehicle, and based on the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, a preliminary position estimate of the vehicle at the current moment can be calculated, that is, the first position information corresponding to the vehicle at the current moment. The second measurement data is the position information of multiple ultra-wideband base stations at the current moment obtained by the ultra-wideband wireless positioning method and the distance information corresponding to each ultra-wideband base station and the vehicle, respectively. The principle of multilateral positioning can be used to combine the known coordinates of multiple ultra-wideband base stations and the distance from the vehicle to each base station to determine the second position information corresponding to the vehicle at the current moment. Based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment, the position information of the vehicle at the current moment is predicted by a factor graph model, the positioning data from two different sources of the inertial measurement unit and the ultra-wideband are integrated, and the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment are associated by using a factor graph model, and fused according to the maximum a posteriori probability principle, and the position of the vehicle at the current moment is estimated to obtain the target position information of the vehicle at the current moment. During the driving process of the vehicle, the first measurement data and the second measurement data are collected in real time, and the first position information and the second position information at each corresponding moment are calculated, and the first position information and the second position information are input into the factor graph model, and the calculation is continuously iterated to update the target position information of the vehicle at each moment, thereby realizing the real-time positioning of the vehicle. Through the above steps, the continuous tracking capability of the inertial measurement unit positioning and the high-precision characteristics of ultra-wideband positioning can be utilized, and the different positioning data sources of the inertial measurement unit and the ultra-wideband can be effectively integrated based on the factor graph model for data fusion, and the mutual dependence and uncertainty information between the two can be utilized to overcome the limitations of a single sensor, improve the overall stability and accuracy of positioning, solve the problem of inaccurate positioning of other positioning technologies in the case of failure of GNSS signals in the prior art, and improve the positioning accuracy of the vehicle in a satellite-denied environment.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

FIG1 is a schematic diagram of an application scenario of an embodiment of the present application;

FIG2 is a flow chart of a vehicle positioning method provided in an embodiment of the present application;

FIG3 is a flow chart of another vehicle positioning method provided in an embodiment of the present application;

FIG4 is a schematic diagram of a flow chart of another vehicle positioning method provided in an embodiment of the present application;

FIG5 is a schematic diagram of a simulation analysis of a vehicle positioning method provided in an embodiment of the present application;

FIG6 is a schematic structural diagram of a vehicle positioning device provided in an embodiment of the present application;

FIG. 7 is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application.

Detailed ways

In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

A vehicle positioning method and device according to an embodiment of the present application will be described in detail below with reference to the accompanying drawings.

Fig. 1 is a schematic diagram of an application scenario of an embodiment of the present application. The method is applied to a factor graph model, including: a first measurement factor node, a second measurement factor node, a first state transfer factor node, a second state transfer factor node, a first variable node, a second variable node and a fusion variable node.

As shown in Figure 1, f(Zi|Xi) in the image is the first measurement factor node, which can be used to characterize the impact of the first position information corresponding to the vehicle at each moment obtained from the inertial measurement unit on the vehicle state, and defines the probability dependency relationship between the measurement data (i.e., the first position information) and the state variable (i.e., the first state prediction amount). The first measurement factor node f(Zi|Xi) is a probability density function that obeys a Gaussian distribution. Through the first measurement factor node, the actual observation data (i.e., the first position information) can be compared and corrected with the predicted value (i.e., the first state prediction amount), thereby updating the system's estimate of the vehicle position.

In the image, f(Zi|Yi) is the second measurement factor node, which can be used to characterize the impact of the second position information corresponding to the vehicle at each moment obtained from the ultra-wideband on the vehicle state, and defines the probabilistic dependency between the measurement data (i.e., the second position information) and the state variable (i.e., the second state prediction amount). The second measurement factor node f(Zi|Yi) is a probability density function that obeys a Gaussian distribution. Through the second measurement factor node, the actual observation data (i.e., the second position information) can be compared and corrected with the predicted value (i.e., the second state prediction amount), thereby updating the system's estimate of the vehicle position.

In the image, f(Xi|Xi-1) is the first state transfer factor node. The first state transfer factor node can be used to characterize how the vehicle state evolves over time in a continuous time interval. It can represent the transition from the state at one moment (for example, X1) to the state at the next moment (for example, X2), reflecting the change law of the vehicle's first state prediction from one moment to the next, taking into account the noise and uncertainty in the process of state change. The first state transfer factor node connects the state first variable nodes of different time steps in the factor graph model, which can represent the probability distribution of state transition and predict the state (position) of the vehicle at the next moment. The first state transfer factor node is a probability density function that obeys Gaussian distribution.

In the image, f(Yi|Yi-1) is the second state transfer factor node. The second state transfer factor node can be used to characterize how the vehicle state evolves over time in a continuous time interval. It can represent the transition from the state at one moment (for example, Y1) to the state at the next moment (for example, Y2), reflecting the change law of the vehicle's second state prediction from one moment to the next, taking into account the noise and uncertainty in the process of state change. The second state transfer factor node connects the state second variable nodes of different time steps in the factor graph model, which can represent the probability distribution of state transition and predict the state (position) of the vehicle at the next moment. The second state transfer factor node is a probability density function that obeys Gaussian distribution.

In the image, X1, X2...Xi are all first variable nodes, and X0 is the GPS data of the vehicle obtained based on the global positioning navigation system at the initial moment. The first variable node is used to represent the unknown variable (first state prediction quantity) of the vehicle, which refers to the position coordinates of the vehicle at different time points in this application. The first variable node is the object to be estimated in the factor graph model. By connecting with the first measurement factor node and the first state transfer factor node, and calculating the influence of the first measurement factor node and the first state transfer factor node on the corresponding first variable node, the best estimate value of the first variable node and its probability distribution can be obtained, and the best estimate value of the vehicle position obtained based on the inertial measurement unit measurement data can be determined.

In the image, Y1, Y2, ... Yi are all second variable nodes, and Y0 is the GPS data of the vehicle obtained based on the global positioning navigation system at the initial moment. The second variable node is used to represent the unknown variable (second state prediction quantity) of the vehicle, which refers to the position coordinates of the vehicle at different time points in this application. The second variable node is the object to be estimated in the factor graph model. By connecting with the second measurement factor node and the second state transfer factor node, and calculating the influence of the second measurement factor node and the second state transfer factor node on the corresponding second variable node, the best estimate value of the second variable node and its probability distribution can be obtained, and the best estimate value of the vehicle position based on the ultra-wideband measurement data can be determined.

In the image, XY1, XY2, ..., XYi are all fusion variable nodes, which are fusion state variables obtained by fusing the first state prediction quantity and the second state prediction quantity. The fusion variable node represents the optimal estimate obtained by the interaction and information fusion of the first variable node and the second variable node in the factor graph model. The fusion variable node is the optimal estimate of the vehicle position that integrates the information from the inertial measurement unit and ultra-wideband in the factor graph model, which can eliminate errors and improve positioning accuracy.

FIG2 is a flow chart of a vehicle positioning method provided in an embodiment of the present application. As shown in FIG2 , the vehicle positioning method includes:

Step 201, obtaining first measurement data and second measurement data of the vehicle at the current moment, wherein the first measurement data is vehicle speed information measured by an inertial measurement unit on the vehicle, and the second measurement data is location information of multiple ultra-wideband base stations and respective distance information corresponding to each ultra-wideband base station and the vehicle.

In some embodiments, an inertial measurement unit (IMU) plays an important role in real-time vehicle positioning. An IMU is installed on the vehicle, and the speed information of the vehicle can be measured based on the IMU. The measured speed information includes angular velocity and acceleration. The core components of the IMU include a gyroscope and an accelerometer. The IMU can monitor the motion state of the vehicle in real time, obtain the acceleration information and angular velocity information of the current vehicle, and integrate the acceleration and angular velocity data to obtain the displacement and steering changes of the vehicle, and obtain the first position information of the vehicle at the current moment.

In some embodiments, multiple ultra-wideband (UWB) base stations are deployed within the driving range of the vehicle, and each UWB base station has a known fixed position coordinate, that is, the location information of the above ultra-wideband base station. A vehicle-mounted UWB tag is installed on the vehicle, and the vehicle-mounted UWB tag transmits or receives an ultra-wideband pulse signal. After each UWB base station receives the signal, it can calculate the distance information corresponding to each ultra-wideband base station and the vehicle by accurately measuring the round-trip flight time or arrival time difference of the signal. The number of the above-mentioned UWB base stations is at least 3. The signal of the vehicle-mounted UWB tag is received by at least three UWB base stations, and the position of the vehicle can be calculated by a geometric algorithm. As the vehicle moves, the vehicle-mounted UWB tag continues to exchange signals with each UWB base station, and continuously updates the positioning data, thereby realizing real-time positioning of the vehicle. Subsequently, the first measurement data and the second measurement data at the current moment can be integrated to achieve higher accuracy and more reliable vehicle positioning in a global positioning navigation system (GNSS) denied environment.

Step 202, based on the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, determine the first position information corresponding to the vehicle at the current moment, and based on the position information of multiple ultra-wideband base stations at the current moment and the distance information corresponding to each ultra-wideband base station and the vehicle, determine the second position information corresponding to the vehicle at the current moment.

In some embodiments, the IMU can measure the acceleration and angular velocity of the vehicle. The displacement of the vehicle along a straight line can be obtained by integrating the acceleration twice, and the steering information of the vehicle can be obtained by integrating the angular velocity. The position of the vehicle can be updated in real time based on the position of the vehicle at the starting time and the displacement information obtained by continuous integration. According to the vehicle speed information measured by the inertial measurement unit on the vehicle at the current time, the first position information corresponding to the vehicle at the current time can be calculated.

In some embodiments, UWB technology is to accurately measure the flight time or arrival time difference between each base station and the vehicle-mounted UWB tag on the vehicle by sending and receiving ultra-wideband pulse signals, thereby calculating the distance information from each base station to the vehicle. If the number of UWB base stations is three, the three-sided positioning method can be used to build a geometric relationship based on the position information of the three base stations and the corresponding distance information from each base station to the vehicle, and solve the second position information of the vehicle at the current moment. Specifically, as shown in Figure 3, three UWB base stations are deployed in a GNSS-denied environment, and the vehicle itself is equipped with a vehicle-mounted UWB tag. When the two-dimensional position coordinates of base station a, base station b and base station c are known to be (x ₀ , y ₀ ), (x ₁ , y ₁ ) and (x ₂ , y ₂ ), respectively, and the distances d ₀ , d ₀ and d ₂ between each base station and the vehicle-mounted UWB tag at the current moment, the second position information (x, y) corresponding to the vehicle at the current moment can be obtained by the following formula.

Subsequently, the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment can be combined through the factor graph model technology, which can further improve the accuracy and robustness of vehicle positioning, complement their respective advantages, reduce positioning errors, and improve the positioning accuracy of vehicles in GNSS-denied environments.

Step 203, based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment, the position information of the vehicle at the current moment is predicted through a factor graph model to obtain the target position information of the vehicle at the current moment. The factor graph model is constructed based on historical vehicle speed information and historical position information of multiple ultra-wideband base stations and each distance information corresponding to each ultra-wideband base station and the historical vehicle.

In some embodiments, the factor graph model is constructed based on historical vehicle speed information and historical position information of multiple ultra-wideband base stations and the distance information corresponding to each ultra-wideband base station and the historical vehicle. The above information can provide a rich data basis for the factor graph model, so that the factor graph model can learn the potential relationship between the vehicle position and the first position information of the vehicle and the second position information of the vehicle. The factor graph model can express the causal relationship and probability distribution between the vehicle position and the first position information of the vehicle and the second position information of the vehicle, which helps to integrate data from different information sources. In theory, the more and more comprehensive the historical data, the stronger the learning ability of the factor graph model, and the prediction accuracy will be improved accordingly.

In some embodiments, the factor graph model integrates the first position information corresponding to the vehicle at each moment, the second position information corresponding to the vehicle at each moment, and the variable state of the vehicle at each moment. The variable state at each moment can be used to characterize the optimal position estimate of the vehicle at each moment. The relationship between the variable states can be captured by the factor graph model. The first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment can be fused by methods such as Bayesian reasoning or maximum a posteriori probability to obtain the best estimate of the vehicle's current position, that is, the target position information of the vehicle at the current moment. By fusing the positioning data of IMU and UWB through the factor graph model, the divergence of IMU positioning errors can be suppressed, and the advantages of the two positioning technologies can be combined to update and optimize the vehicle's position information in real time. It is suitable for vehicle positioning in GNSS-denied environments and improves the accuracy and stability of vehicle positioning.

Based on the vehicle positioning method provided by the present application, by obtaining the first measurement data and the second measurement data of the vehicle at the current moment, the first measurement data is the vehicle speed information measured by the inertial measurement unit on the vehicle, and the inertial measurement unit can provide the acceleration and angular velocity of the vehicle in real time. Based on the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, a preliminary position estimate of the vehicle at the current moment can be calculated, that is, the first position information corresponding to the vehicle at the current moment. The second measurement data is the position information of multiple ultra-wideband base stations at the current moment obtained by the ultra-wideband wireless positioning method and the respective distance information corresponding to each ultra-wideband base station and the vehicle. The principle of multilateral positioning can be used, combined with the known coordinates of multiple ultra-wideband base stations and the distance from the vehicle to each base station, to determine the position of the vehicle, that is, the second position information corresponding to the vehicle at the current moment. Based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment, the position information of the vehicle at the current moment is predicted by the factor graph model, and the positioning data from two different sources, the inertial measurement unit and the ultra-wideband, are integrated. The first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment are associated by the factor graph model, and are fused according to the maximum a posteriori probability principle, and the position of the vehicle at the current moment is estimated to obtain the target position information of the vehicle at the current moment. During the driving process of the vehicle, the first measurement data and the second measurement data are collected in real time, and the corresponding first position information and second position information at each moment are calculated, and the first position information and the second position information are input into the factor graph model, and the calculation is continuously iterated to update the target position information of the vehicle at each moment, so as to realize the real-time positioning of the vehicle. The factor graph model is constructed and trained based on the historical vehicle speed information and the historical position information of multiple ultra-wideband base stations, and the distance information corresponding to each ultra-wideband base station and the historical vehicle. Through the above steps, the continuous tracking capability of the inertial measurement unit and the high-precision characteristics of ultra-wideband positioning can be utilized, and the different positioning data sources of the inertial measurement unit and ultra-wideband can be effectively integrated for data fusion based on the factor graph model. The mutual dependence and uncertainty information between the two can be utilized to overcome the limitations of a single sensor, which not only suppresses the error accumulation problem after long-term use of the inertial measurement unit, but also improves the overall stability and accuracy of positioning, solves the problem of inaccurate positioning of other positioning technologies when the GNSS signal fails in the existing technology, and improves the positioning accuracy of vehicles in satellite denial environments.

In some embodiments, before predicting the position information of the vehicle at the current moment through the factor graph model, it also includes: obtaining the first measurement data of the vehicle at each historical moment and the second measurement data at each historical moment; constructing a first measurement factor node and a second measurement factor node based on the measurement equation, the first measurement factor node connects the first measurement data of the vehicle at each historical moment and the corresponding first variable node, and the second measurement factor node connects the second measurement data of the vehicle at each historical moment and the corresponding second variable node; using the position information of the vehicle at each historical moment as a fusion variable node, constructing a first state transfer factor node and a second state transfer factor node based on the state equation, the first state transfer factor node connects the first variable nodes of the previous and next historical moments, the second state transfer factor node connects the second variable nodes of the previous and next historical moments, and the fusion variable node is obtained by fusing the corresponding first state transfer factor node and the second state transfer factor node; constructing a factor graph model according to the first measurement factor node, the second measurement factor node, the first state transfer factor node, the second state transfer factor node, the first variable node, the second variable node and the fusion variable node.

In some embodiments, obtaining the first measurement data and the second measurement data of the vehicle at each historical moment is a basic step in constructing a factor graph model. The first measurement data of the vehicle at each historical moment is obtained by measuring with an inertial measurement unit, which may be the acceleration and angular velocity of the vehicle at each historical moment. The second measurement data of the vehicle at each historical moment is obtained by UWB measurement, that is, the measured value of the distance between the vehicle and multiple UWB base stations. The position of the vehicle can be calculated in combination with the known positions of each base station. The first measurement data and the second measurement data provide key information about the position and motion state of the vehicle. The first measurement data and the second measurement data have different precision and characteristics, and using them in combination can improve the accuracy and robustness of position estimation.

In some embodiments, a first measurement factor node and a second measurement factor node are constructed based on a measurement equation, and the measurement equation includes an inertial measurement unit state equation and an ultra-wideband state equation. The inertial measurement unit state equation is: Z _IMU = H _IMU X + K _IMU V _IMU , where Z _IMU is a measurement, i.e., first measurement data, H _IMU is a measurement matrix, K _IMU is a noise matrix based on the inertial measurement unit, V _IMU is an error noise based on the inertial measurement unit, and X represents the vehicle state estimated based on the first measurement data at the current moment. The ultra-wideband state equation is: Z _UWB = H _UWB Y + K _UWB V _UWB , where Z _UWB is a measurement, i.e., second measurement data, H _UWB is a measurement matrix, K _UWB is a noise matrix based on ultra-wideband, V _UWB is an error noise based on ultra-wideband, and Y represents the vehicle state estimated based on the second measurement data at the current moment.

In some embodiments, a first measurement factor node is constructed through an inertial measurement unit state equation, and a connection is established between the first measurement data of the vehicle at each historical moment and the corresponding first variable node in the factor graph model. The first measurement factor node can represent the constraint relationship between the first measurement data of the vehicle at each historical moment and the corresponding first variable node, and the first measurement data is integrated into the entire model through the structure of the factor graph model. A second measurement factor node is constructed through an ultra-wideband state equation, and a connection is established between the second measurement data of the vehicle at each historical moment and the corresponding second variable node in the factor graph model. The second measurement factor node can represent the constraint relationship between the second measurement data of the vehicle at each historical moment and the corresponding second variable node, and the second measurement data is integrated into the entire model through the structure of the factor graph model. The above-mentioned first variable node corresponds to the vehicle state estimated based on the first measurement data, and the second variable node corresponds to the vehicle state estimated based on the second measurement data.

In some embodiments, the position information of the vehicle at each historical moment is used as a fusion variable node, and the first state transfer factor node and the second state transfer factor node are constructed based on the state equation, so that the continuity and change of the vehicle state can be captured in the factor graph model. The first state transfer factor node can be used to characterize the state transfer process calculated according to the first measurement data in the time series of the vehicle, and describe the law of the vehicle state changing over time. The first state transfer factor node links the first variable node of the vehicle based on the first measurement data at the adjacent moment. The second state transfer factor node can be used to characterize the state transfer process calculated according to the second measurement data in the time series of the vehicle, and describe the law of the vehicle state changing over time. The second state transfer factor node links the second variable node of the vehicle based on the second measurement data at the adjacent moment. The fusion variable node is a fusion of the corresponding first variable node and the second variable node. The global optimal position of the vehicle at the current moment can be estimated by the maximum a posteriori probability estimation method, taking into account the changing law of the vehicle motion state and the measurement data from multiple sources. The solution of the fusion variable node, that is, the estimated target position information of the vehicle, helps to more comprehensively predict the state of the vehicle and improve the accuracy of the position estimation.

In some embodiments, a complete factor graph model is constructed based on the above-mentioned first measurement factor node, second measurement factor node, first state transfer factor node, second state transfer factor node, first variable node, second variable node and fusion variable node. The factor graph model can comprehensively consider the measurement data, state transition and the dependency between variables to achieve accurate estimation and prediction of vehicle position information.

In some embodiments, based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment, the position information of the vehicle at the current moment is predicted through a factor graph model to obtain the target position information of the vehicle at the current moment, including: calculating and processing the first position information corresponding to the current moment through the first measurement factor node, the first state transfer factor node and the first state quantity of the vehicle at the previous moment to obtain the first state prediction quantity of the vehicle corresponding to the inertial measurement unit at the current moment; calculating and processing the second position information corresponding to the current moment through the second measurement factor node, the second state transfer factor node and the second state quantity of the vehicle at the previous moment to obtain the second state prediction quantity of the vehicle corresponding to the inertial measurement unit at the current moment; fusing the first state prediction quantity of the vehicle corresponding to the inertial measurement unit at the current moment and the second state prediction quantity of the vehicle corresponding to the ultra-wideband at the current moment to obtain the target position information of the vehicle at the current moment.

In some embodiments, the first position information corresponding to the current moment is calculated and processed through the first measurement factor node, the first state transfer factor node and the first state quantity of the vehicle at the previous moment in the factor graph model. The first measurement factor node reflects the constraint relationship between the first position information obtained by the inertial measurement unit and the first state quantity of the vehicle. The first state transfer factor node describes the change law of the vehicle state over time. The mean and variance corresponding to the first variable node corresponding to the current moment are calculated through historical data (i.e., the first state quantity of the vehicle at the previous moment) and the current measurement information (i.e., the first position information). The mean and variance corresponding to the first variable node corresponding to the current moment can represent the optimal estimate of the state of the vehicle at the current moment based on the inertial measurement unit measurement, and obtain the vehicle first state prediction quantity of the inertial measurement unit at the current moment, thereby realizing the prediction and update of the vehicle state. When the first position information of the vehicle at each moment is known, the vehicle first state prediction quantity of the inertial measurement unit at each moment is predicted, which can be predicted by the following formula:

Wherein X ₁ , X ₂ , …, X _n are the first state predictions of the vehicle about the inertial measurement unit at each time, Z ₁ , Z ₂ , …, Z _n are the first position information of the vehicle at each time, f(X _i |X _i-1 ) is the first state transfer factor node, f(Z _i |X _i ) is the first measurement factor node, and the above f(X _i |X _i-1 ) obeys Gaussian distribution, and its mean and variance are Mi _-1 Xi _-1 and f(Z _i |X _i ) obeys Gaussian distribution, and its mean and variance are H _i X _i and /> The above M is a matrix, H is a measurement matrix, and K is a noise matrix based on an inertial measurement unit.

The first position information corresponding to the current moment is calculated and processed through the first measurement factor node, the first state transfer factor node and the first state quantity of the vehicle at the previous moment, so as to obtain the first state prediction quantity of the vehicle corresponding to the current moment about the inertial measurement unit, which can be represented by the formula:

in

The above Pi _|i ( _Xi ) is the predicted value of the first state of the vehicle corresponding to the inertial measurement unit at the current moment, Pi _|i-1 ( _Xi ) is the first state of the vehicle at the previous moment, and f( _Zi | _Yi ) is the first measurement factor node.

Similarly, the first position information corresponding to the next moment is calculated and processed through the first measurement factor node, the first state transfer factor node and the first state of the vehicle at the current moment, and the predicted value of the first state of the vehicle corresponding to the next moment about the inertial measurement unit is obtained, which can be represented by the formula:

In some embodiments, the second position information corresponding to the current moment is calculated and processed through the second measurement factor node, the second state transfer factor node and the second state quantity of the vehicle at the previous moment in the factor graph model. The second measurement factor node reflects the constraint relationship between the second position information obtained by ultra-wideband measurement and the second state quantity of the vehicle. The second state transfer factor node describes the change law of the vehicle state over time. The mean and variance corresponding to the second variable node corresponding to the current moment are calculated through historical data (i.e., the second state quantity of the vehicle at the previous moment) and the current measurement information (i.e., the second position information). The mean and variance corresponding to the second variable node corresponding to the current moment can represent the optimal estimate of the state of the vehicle at the current moment based on the ultra-wideband measurement, and obtain the vehicle second state prediction quantity about the ultra-wideband at the current moment, thereby realizing the prediction and update of the vehicle state.

In some embodiments, the first state prediction of the vehicle corresponding to the inertial measurement unit at the current moment (the mean and variance corresponding to the first variable node at the current moment) and the second state prediction of the vehicle corresponding to the ultra-wideband at the current moment (the mean and variance corresponding to the second variable node at the current moment) are fused using a factor graph model. Considering the correlation and uncertainty between different information sources, the posterior probability or other fusion algorithms can be maximized, and the first state prediction and the second state prediction are integrated. According to factors such as the reliability of the first state prediction and the second state prediction, different weights can be assigned to the first state prediction and the second state prediction, respectively, to estimate the optimal position of the vehicle, and the optimal position is the target predicted position of the vehicle. Specifically, the first state quantity of the vehicle at the initial moment can be X0, and the second state quantity of the vehicle at the initial moment can be Y0. The first state quantity of the vehicle at the initial moment and the second state quantity of the vehicle at the initial moment are both the positioning data of the GPS just entering the GNSS denied environment at the initial moment. The first position information corresponding to the first moment is calculated and processed through the first measurement factor node, the first state transfer factor node and the first state quantity X0 of the vehicle at the initial moment, and the first state prediction quantity X1 of the vehicle corresponding to the inertial measurement unit at the first moment is obtained; the second position information corresponding to the first moment is calculated and processed through the second measurement factor node, the second state transfer factor node and the second state quantity Y0 of the vehicle at the initial moment, and the second state prediction quantity Y1 of the vehicle corresponding to the inertial measurement unit at the first moment is obtained; the target position information XY1 of the vehicle at the first moment is obtained by fusing the first state prediction quantity X1 of the vehicle corresponding to the inertial measurement unit at the first moment and the second state prediction quantity Y1 of the vehicle corresponding to the inertial measurement unit at the first moment. In the vehicle positioning method based on the factor graph model of the present application, the continuity and stability of the long-term tracking of the inertial measurement unit are considered, and the real-time and relatively accurate position information of the ultra-wideband is integrated, so as to improve the accuracy and robustness of the real-time positioning of the vehicle.

In some embodiments, multiple position information measured by multiple sensors at the current moment of the vehicle can also be obtained. The multiple position information measured by multiple sensors (vehicle positioning devices, such as inertial measurement units, ultra-wideband, laser radar, ultrasonic waves, etc.) at the current moment of the vehicle is Y ₁ , Y ₂ ,…, Y _n . According to Gaussian distribution, the measurement model is: The factor graph model can be used to fuse multiple location information measured by multiple sensors to predict the location information of the vehicle at the current moment. The formula for predicting the location information of the vehicle from time i-1 to time i based on the factor graph model can be:

P(X _ij |Y _ij )＝P(X _ij |X _i-1|j )P(Y _ij |X _i )

Based on the factor graph model, multiple position information measured by multiple sensors at the current moment is fused, and the predicted position information of the vehicle at the current moment can be expressed as: Above/> The vehicle's current position information is predicted. By integrating the position information measured and calculated by multiple sensors (vehicle real-time positioning devices), the vehicle is positioned in real time, improving the overall positioning accuracy and robustness.

In some embodiments, the first position information corresponding to the current moment is calculated and processed through the first measurement factor node, the first state transfer factor node and the first state quantity of the vehicle at the previous moment to obtain the vehicle first state prediction quantity corresponding to the inertial measurement unit at the current moment, including: predicting the first state quantity of the vehicle at the current moment according to the first state quantity of the vehicle at the previous moment and the first state transfer factor node to obtain the vehicle first initial state prediction quantity of the vehicle at the current moment; performing error calculation based on the vehicle first initial state prediction quantity of the vehicle at the current moment and the first position information corresponding to the current moment to obtain the first loss; updating the first measurement factor node according to the first loss to obtain the updated first measurement factor node; updating the first state transfer factor node based on the first state quantity of the vehicle at the previous moment and the first position information corresponding to the current moment to obtain the updated first state transfer factor node; calculating and processing the first position information corresponding to the current moment through the updated first measurement factor node to obtain the vehicle first state prediction quantity corresponding to the inertial measurement unit at the current moment.

In some embodiments, the first state quantity of the vehicle at the current moment is predicted based on the first state quantity of the vehicle at the previous moment and the first state transfer factor node, and the vehicle state at the current moment is predicted using the known first state transfer factor node and the first state quantity of the vehicle at the previous moment, to obtain a preliminary, uncorrected prediction of the first initial state of the vehicle. The error calculation is performed between the predicted value of the first initial state of the current vehicle obtained based on the state transfer prediction and the first position information corresponding to the current moment (i.e., the vehicle position measured by the actual IMU), and the difference between the two is calculated to obtain the first loss. The first loss can be used to update the first measurement factor node later to improve the accuracy of the factor graph model prediction. According to the calculated first loss, the first measurement factor node is updated, and the first measurement factor node can characterize the degree of influence of the IMU measurement data (first position information) on the vehicle state. After the update, the first measurement factor node will more accurately describe the relationship between the IMU measurement data (first position information) and the vehicle state. The first state transfer factor node is updated based on the first state quantity of the vehicle at the previous moment and the first position information corresponding to the current moment, and the first state transfer factor node is optimized to better reflect the actual situation of the change of the first state quantity of the vehicle over time. After obtaining the updated first measurement factor node and the updated first state transfer factor node, the first position information corresponding to the current moment is calculated and estimated based on the updated first measurement factor node to obtain a more accurate prediction of the vehicle's first state corresponding to the current moment regarding the inertial measurement unit. By continuously looping the above steps, the factor graph model is continuously updated according to the new first position information during the real-time positioning process, and can track and estimate the vehicle's first state prediction at each moment in real time and accurately. Through the continuous iteration and continuous updating of the factor graph model in the above process, the first position information is combined with the first state transfer factor node, the positioning error is gradually reduced, and the positioning accuracy is improved.

In some embodiments, the second position information corresponding to the current moment is calculated and processed through the second measurement factor node, the second state transfer factor node and the second state of the vehicle at the previous moment to obtain the vehicle second state prediction quantity corresponding to the inertial measurement unit at the current moment, including: predicting the second state of the vehicle at the current moment according to the second state of the vehicle at the previous moment and the second state transfer factor node to obtain the vehicle second initial state prediction quantity of the vehicle at the current moment; performing error calculation based on the vehicle second initial state prediction quantity of the vehicle at the current moment and the second position information corresponding to the current moment to obtain the second loss; updating the second measurement factor node according to the second loss to obtain an updated second measurement factor node; updating the second state transfer factor node based on the second state of the vehicle at the previous moment and the second position information corresponding to the current moment to obtain an updated second state transfer factor node; calculating and processing the second position information corresponding to the current moment through the updated second measurement factor node to obtain the vehicle second state prediction quantity corresponding to the inertial measurement unit at the current moment.

In some embodiments, the second state quantity of the vehicle at the current moment is predicted based on the second state quantity of the vehicle at the previous moment and the second state transfer factor node, and the vehicle state at the current moment is predicted using the known second state transfer factor node and the second state quantity of the vehicle at the previous moment, to obtain a preliminary, uncorrected prediction of the second initial state of the vehicle. The error calculation is performed on the predicted second initial state quantity of the current vehicle obtained based on the state transfer prediction and the second position information corresponding to the current moment (i.e., the actual vehicle position measured by UWB), and the difference between the two is calculated to obtain the second loss. The second loss can be used to update the second measurement factor node later to improve the accuracy of the factor graph model prediction. According to the calculated second loss, the second measurement factor node is updated, and the second measurement factor node can characterize the degree of influence of the UWB measurement data (second position information) on the vehicle state. After the update, the second measurement factor node will more accurately describe the relationship between the UWB measurement data (second position information) and the vehicle state. The second state transfer factor node is updated based on the second state quantity of the vehicle at the previous moment and the second position information corresponding to the current moment, and the second state transfer factor node is optimized to better reflect the actual situation of the second state quantity of the vehicle changing over time. After obtaining the updated second measurement factor node and the updated second state transfer factor node, the second position information corresponding to the current moment is calculated and estimated based on the updated second measurement factor node to obtain a more accurate prediction of the vehicle's second state corresponding to the current moment regarding the inertial measurement unit. By continuously looping the above steps, the factor graph model is continuously updated according to the new second position information during the real-time positioning process, and can track and estimate the vehicle's second state prediction at each moment in real time and accurately. Through the continuous iteration and continuous updating of the factor graph model in the above process, the second position information is combined with the second state transfer factor node, the positioning error is gradually reduced, and the positioning accuracy is improved.

In some embodiments, the vehicle speed information includes the acceleration at the current moment and the angular velocity at the current moment. According to the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, the first position information corresponding to the vehicle at the current moment is determined, including: integrating the acceleration at the current moment to obtain the displacement of the vehicle; integrating the angular velocity at the current moment to obtain the steering information of the vehicle; based on the displacement of the vehicle and the steering information of the vehicle, obtaining the first position information corresponding to the vehicle at the current moment.

In some embodiments, the vehicle speed information includes the acceleration at the current moment and the angular velocity at the current moment, and both the acceleration and the angular velocity are key parameters for describing the vehicle's motion state. Acceleration can reflect the speed of the vehicle's speed change, and the angular velocity can reflect the speed of the vehicle's rotation. The acceleration and angular velocity of the vehicle at the current moment can be measured by the inertial measurement unit. When the inertial measurement unit on the vehicle measures the acceleration at the current moment, the acceleration at the current moment is integrated twice to obtain the linear displacement of the vehicle during this period. Acceleration is the rate of change of velocity per unit time. Two consecutive integrations can convert acceleration into velocity and then into displacement. By performing two integration operations on the acceleration at the current moment, the displacement of the vehicle in a certain direction within a certain period of time can be estimated. When the inertial measurement unit on the vehicle measures the angular velocity at the current moment, the angular velocity at the current moment is integrated to obtain the amount of change in the vehicle's angle within the time period, that is, the steering information of the vehicle, and the angle information is used to determine the direction change of the vehicle based on straight-line driving. After obtaining the vehicle's linear displacement and steering information, the displacement can be added to the position of the vehicle at the previous moment by superposition or vector synthesis, and the vehicle's orientation can be adjusted according to the steering information. The new position information of the vehicle at the current moment relative to the position at the previous moment, that is, the first position information of the vehicle corresponding to the current moment, can be calculated.

In some embodiments, a first state transfer factor node and a second state transfer factor node are constructed based on a state equation, including: performing Gaussian distribution on a first state quantity of the vehicle at adjacent moments to obtain a first Gaussian distribution result; constructing a first state transfer factor node based on the first Gaussian distribution result and the state equation of an inertial measurement unit, the first state transfer factor node representing a probability distribution of state transfer from one moment to the next; performing Gaussian distribution on a second state quantity of the vehicle at adjacent moments to obtain a second Gaussian distribution result; constructing a second state transfer factor node based on the second Gaussian distribution result and the ultra-wideband state equation, the second state transfer factor node representing a probability distribution of state transfer from one moment to the next.

In some embodiments, the state equation includes an inertial measurement unit state equation and an ultra-wideband state equation. The inertial measurement unit state equation is Xi= _MIMU X(i-1)+ _NIMU W _IMU , where Xi is the first state quantity of the vehicle at the current moment, and is the optimal estimate of the first state quantity of the vehicle at the i-th moment (current moment). The above X(i-1) is the first state quantity of the vehicle at the previous moment, that is, the first state quantity of the vehicle at the i-1th moment. _MIMU is a matrix that reflects the evolution law of the first state of the vehicle over time. _NIMU is an IMU noise matrix that is used to represent the influence of IMU noise on the first state transfer of the vehicle. W _IMU is IMU noise, which can represent the uncertainty in the transfer process of the first state of the vehicle. Based on the factor graph model, various data in the process of vehicle driving are modeled, and the first state quantity of the vehicle at each moment is defined as each first variable node. The first state transfer from one moment to the next adjacent moment obeys Gaussian distribution. The first variable nodes at each moment are solved to obtain the mean and variance of the first state quantity at each moment. The first state transfer is random, so the change of the first state of the vehicle can be regarded as a probabilistic event. In the factor graph model, corresponding to the process of the vehicle's first state transfer, a first state transfer factor node can be constructed, which reflects the probability dependency between the two first state quantities of the vehicle's adjacent time steps that evolve over time. Gaussian distribution is a commonly used probability distribution model that can be used to describe the distribution of random variables. By performing Gaussian distribution on the first state quantities of the vehicle at adjacent moments (i.e., the previous moment and the adjacent next moment), the probability distribution of the first state quantity between different moments can be obtained, i.e., the first Gaussian distribution result. The first state transfer factor node is constructed based on the first Gaussian distribution result and the inertial measurement unit state equation.

In some embodiments, the ultra-wideband state equation is Yi=M _UWB Y(i-1)+N _UWB W _UWB , where Yi is the second state quantity of the vehicle at the current moment, and is the optimal estimate of the second state quantity of the vehicle at the i-th moment (current moment), Y(i-1) is the second state quantity of the vehicle at the previous moment, that is, the second state quantity of the vehicle at the i-1th moment, M _UWB is a matrix reflecting the evolution law of the second state of the vehicle over time, N _UWB is a UWB noise matrix, used to represent the influence of UWB noise on the second state transition of the vehicle, and W _UWB is UWB noise, which can represent the uncertainty in the transition process of the second state of the vehicle. Based on the factor graph model, various data in the vehicle driving process are modeled, and the second state quantity of the vehicle at each moment is defined as each second variable node. The second state transition from one moment to the next adjacent moment obeys Gaussian distribution. The second variable nodes at each moment are solved to obtain the mean and variance of the second state quantity at each moment. The second state transition is random, so the change of the second state of the vehicle can be regarded as a probabilistic event. In the factor graph model, corresponding to the process of the vehicle's second state transfer, a second state transfer factor node can be constructed, which reflects the probability dependency relationship between the two second state quantities of the vehicle's adjacent time steps that evolve over time. By performing Gaussian distribution on the second state quantities of the vehicle at adjacent moments (i.e., the previous moment and the adjacent next moment), the probability distribution of the second state quantity at different moments can be obtained, i.e., the second Gaussian distribution result. The second state transfer factor node is constructed based on the second Gaussian distribution result and the ultra-wideband state equation.

Referring to FIG4 , a flow chart of a vehicle positioning method is shown in FIG4 , and the steps include:

Step 401, initializing the measurement information of the inertial measurement unit and the ultra-wideband;

Step 402, establishing a factor graph model;

Step 403, input the first location information and the second location information, if not, execute step 406;

Step 404, calculating the mean and variance of the first variable node and the mean and variance of the second variable node;

Step 405, merging the first variable node and the second variable node;

Step 406, stop data updating and keep the original data.

In the above steps, in step 401, the IMU data and UWB data required for vehicle positioning are initialized, and the positioning calculation is ready to start. IMU provides information such as acceleration and angular velocity of the vehicle, and UWB provides accurate distance information between the vehicle and the UWB base station and the location information of each UWB base station. In step 402, a factor graph model framework for data fusion and positioning optimization is constructed. The factor graph model can integrate IMU data and UWB data, and express the correlation and uncertainty between IMU data and UWB data through the Bayesian network structure, which is convenient for efficient probabilistic reasoning and optimal state estimation of the vehicle. In step 404, the first position information (the vehicle position information calculated based on the IMU measurement data) and the second position information (the vehicle position information calculated based on the UWB measurement data) are input into the factor graph model. If there is no new data, step 406 is executed. In step 404, the confidence and central tendency of the vehicle position estimation based on the two sensor data are quantified by calculating the mean and variance of the first variable node and the second variable node. This step is an important step for probabilistic fusion and optimization, and provides basic data for subsequent fusion processing. In step 405, the first variable node and the second variable node are fused with information through a factor graph model, and an optimal state estimation is performed through a probability theory method (such as Bayesian filtering or particle filtering), so as to obtain more accurate target position information of the vehicle.

The vehicle positioning proposed in this application is simulated and analyzed, and compared with the method using only IMU positioning, the simulation software used is Matlab. After setting the real motion trajectory of the vehicle, two methods are used to simulate the motion trajectory of the vehicle to obtain the predicted motion trajectory of the vehicle. The x-axis error and y-axis error of the predicted trajectory and the real trajectory in the two-dimensional plane are calculated, and the errors of the x-axis and y-axis are compared and analyzed to obtain an error schematic diagram as shown in Figure 5. The absolute values of the x-axis and y-axis errors obtained by the method using only IMU positioning are 0.98m and 1.12m respectively, and the absolute values of the x-axis and y-axis errors after IMU/UWB data fusion (positioning method of this application) are 0.38m and 0.31m respectively. Therefore, the method proposed in this application has a smaller error than the result obtained by using only IMU positioning, and the positioning accuracy is higher.

All the above optional technical solutions can be arbitrarily combined to form optional embodiments of the present application, which will not be described one by one here.

The following are device embodiments of the present application, which can be used to execute the method embodiments of the present application. For details not disclosed in the device embodiments of the present application, please refer to the method embodiments of the present application.

FIG6 is a schematic diagram of a vehicle positioning device provided in an embodiment of the present application. As shown in FIG5 , the vehicle positioning device includes:

An acquisition module 601 is used to acquire first measurement data and second measurement data of the vehicle at a current moment, wherein the first measurement data is vehicle speed information measured by an inertial measurement unit on the vehicle, and the second measurement data is location information of multiple ultra-wideband base stations and respective distance information corresponding to each ultra-wideband base station and the vehicle;

The calculation module 602 is used to determine the first position information corresponding to the vehicle at the current moment according to the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, and determine the second position information corresponding to the vehicle at the current moment according to the position information of multiple ultra-wideband base stations at the current moment and the respective distance information corresponding to each ultra-wideband base station and the vehicle;

The prediction module 603 is used to predict the position information of the vehicle at the current moment based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment through a factor graph model to obtain the target position information of the vehicle at the current moment. The factor graph model is constructed based on historical vehicle speed information and historical position information of multiple ultra-wideband base stations and various distance information corresponding to each ultra-wideband base station and historical vehicles.

According to the technical solution provided in the embodiment of the present application, the first measurement data and the second measurement data of the vehicle at the current moment are obtained by the acquisition module 601. The first measurement data is the vehicle speed information measured by the inertial measurement unit on the vehicle. The inertial measurement unit can provide the acceleration and angular velocity of the vehicle in real time. The calculation module 602 can calculate a preliminary position estimate of the vehicle at the current moment based on the vehicle speed information measured by the inertial measurement unit on the vehicle at the current moment, that is, the first position information corresponding to the vehicle at the current moment. The second measurement data is the position information of multiple ultra-wideband base stations at the current moment obtained by the ultra-wideband wireless positioning method and the distance information corresponding to each ultra-wideband base station and the vehicle. The calculation module 602 can use the principle of multilateral positioning, combined with the known coordinates of multiple ultra-wideband base stations and the distance from the vehicle to each base station, to determine the position of the vehicle, that is, the second position information corresponding to the vehicle at the current moment. Based on the first position information corresponding to the vehicle at the current moment and the second position information corresponding to the vehicle at the current moment, the prediction module 603 predicts the position information of the vehicle at the current moment, integrates the positioning data from two different sources, the inertial measurement unit and the ultra-wideband, and uses the factor graph model to associate the first position information corresponding to the vehicle at the current moment with the second position information corresponding to the vehicle at the current moment, and fuses them according to the maximum a posteriori probability principle, estimates the position of the vehicle at the current moment, and obtains the target position information of the vehicle at the current moment. During the driving process of the vehicle, the first measurement data and the second measurement data are collected in real time, and the corresponding first position information and second position information at each moment are calculated, and the first position information and the second position information are input into the factor graph model, and the calculation is continuously iterated to update the target position information of the vehicle at each moment, so as to realize the real-time positioning of the vehicle. The factor graph model is constructed and trained based on the historical vehicle speed information and the historical position information of multiple ultra-wideband base stations and the distance information corresponding to each ultra-wideband base station and the historical vehicle. Through the above steps, the continuous tracking capability of the inertial measurement unit and the high-precision characteristics of ultra-wideband positioning can be utilized, and the different positioning data sources of the inertial measurement unit and ultra-wideband can be effectively integrated for data fusion based on the factor graph model. The mutual dependence and uncertainty information between the two can be utilized to overcome the limitations of a single sensor, which not only suppresses the error accumulation problem after long-term use of the inertial measurement unit, but also improves the overall stability and accuracy of positioning, solves the problem of inaccurate positioning of other positioning technologies when the GNSS signal fails in the existing technology, and improves the positioning accuracy of vehicles in satellite denial environments.

In some embodiments, before predicting the position information of the vehicle at the current moment through the factor graph model, the vehicle positioning device is configured to obtain the first measurement data of the vehicle at each historical moment and the second measurement data of each historical moment; construct a first measurement factor node and a second measurement factor node based on the measurement equation, the first measurement factor node connects the first measurement data of the vehicle at each historical moment and the corresponding first variable node, and the second measurement factor node connects the second measurement data of the vehicle at each historical moment and the corresponding second variable node; use the position information of the vehicle at each historical moment as a fusion variable node, and construct a first state transfer factor node and a second state transfer factor node based on the state equation, the first state transfer factor node connects the first variable nodes of the previous and next historical moments, the second state transfer factor node connects the second variable nodes of the previous and next historical moments, and the fusion variable node is obtained by fusing the corresponding first state transfer factor node and the second state transfer factor node; construct a factor graph model according to the first measurement factor node, the second measurement factor node, the first state transfer factor node, the second state transfer factor node, the first variable node, the second variable node and the fusion variable node.

In some embodiments, the prediction module 603 is configured to calculate and process the first position information corresponding to the current moment through the first measurement factor node, the first state transfer factor node and the first state of the vehicle at the previous moment, and obtain the first state prediction of the vehicle corresponding to the inertial measurement unit at the current moment; calculate and process the second position information corresponding to the current moment through the second measurement factor node, the second state transfer factor node and the second state of the vehicle at the previous moment, and obtain the second state prediction of the vehicle corresponding to the inertial measurement unit at the current moment; perform fusion processing based on the first state prediction of the vehicle corresponding to the inertial measurement unit at the current moment and the second state prediction of the vehicle corresponding to the ultra-wideband at the current moment, and obtain the target position information of the vehicle at the current moment.

In some embodiments, the prediction module 603 is configured to predict the first state quantity of the vehicle at the current moment based on the first state quantity of the vehicle at the previous moment and the first state transfer factor node, and obtain the first initial state prediction quantity of the vehicle at the current moment; perform error calculation based on the first initial state prediction quantity of the vehicle at the current moment and the first position information corresponding to the current moment, and obtain the first loss; update the first measurement factor node according to the first loss, and obtain the updated first measurement factor node; update the first state transfer factor node based on the first state quantity of the vehicle at the previous moment and the first position information corresponding to the current moment, and obtain the updated first state transfer factor node; calculate and process the first position information corresponding to the current moment through the updated first measurement factor node, and obtain the first state prediction quantity of the vehicle corresponding to the inertial measurement unit at the current moment.

In some embodiments, the prediction module 603 is configured to predict the second state quantity of the vehicle at the current moment based on the second state quantity of the vehicle at the previous moment and the second state transfer factor node, and obtain the vehicle second initial state prediction quantity of the vehicle at the current moment; perform error calculation based on the vehicle second initial state prediction quantity of the vehicle at the current moment and the second position information corresponding to the current moment, and obtain the second loss; update the second measurement factor node according to the second loss, and obtain an updated second measurement factor node; update the second state transfer factor node based on the second state quantity of the vehicle at the previous moment and the second position information corresponding to the current moment, and obtain an updated second state transfer factor node; calculate and process the second position information corresponding to the current moment through the updated second measurement factor node, and obtain the vehicle second state prediction quantity corresponding to the inertial measurement unit at the current moment.

In some embodiments, the vehicle speed information includes the acceleration at the current moment and the angular velocity at the current moment. The calculation module 602 is configured to integrate the acceleration at the current moment to obtain the displacement of the vehicle; integrate the angular velocity at the current moment to obtain the steering information of the vehicle; based on the displacement of the vehicle and the steering information of the vehicle, obtain the first position information corresponding to the vehicle at the current moment.

In some embodiments, the state equation includes an inertial measurement unit state equation and an ultra-wideband state equation, and the vehicle positioning device is configured to perform Gaussian distribution on the first state quantity of the vehicle at adjacent moments to obtain a first Gaussian distribution result; construct a first state transfer factor node based on the first Gaussian distribution result and the inertial measurement unit state equation, and the first state transfer factor node represents the probability distribution of state transfer from one moment to the next moment; perform Gaussian distribution on the second state quantity of the vehicle at adjacent moments to obtain a second Gaussian distribution result; construct a second state transfer factor node based on the second Gaussian distribution result and the ultra-wideband state equation, and the second state transfer factor node represents the probability distribution of state transfer from one moment to the next moment.

It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

FIG7 is a schematic diagram of an electronic device 7 provided in an embodiment of the present application. As shown in FIG7 , the electronic device 7 of this embodiment includes: a processor 701, a memory 702, and a computer program 703 stored in the memory 702 and executable on the processor 701. When the processor 701 executes the computer program 703, the steps in the above-mentioned various method embodiments are implemented. Alternatively, when the processor 701 executes the computer program 703, the functions of each module/unit in the above-mentioned various device embodiments are implemented.

The electronic device 7 may be a desktop computer, a notebook, a PDA, a cloud server, or other electronic device. The electronic device 7 may include, but is not limited to, a processor 701 and a memory 702. Those skilled in the art will appreciate that FIG. 7 is merely an example of the electronic device 7 and does not limit the electronic device 7, and may include more or fewer components than shown in the figure, or different components.

The processor 701 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

The memory 702 may be an internal storage unit of the electronic device 7, for example, a hard disk or memory of the electronic device 7. The memory 702 may also be an external storage device of the electronic device 7, for example, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the electronic device 7. The memory 702 may also include both an internal storage unit and an external storage device of the electronic device 7. The memory 702 is used to store computer programs and other programs and data required by the electronic device.

Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the device is divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units.

If the integrated module/unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a readable storage medium (e.g., a computer-readable storage medium). Based on this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. The computer program may include computer program code, which may be in source code form, object code form, executable file or some intermediate form. Computer-readable storage media may include: any entity or device capable of carrying computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium, etc.

The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims (10)

1. A vehicle positioning method, characterized by comprising:

Acquiring first measurement data and second measurement data of a vehicle at the current moment, wherein the first measurement data are vehicle speed information measured by an inertial measurement unit on the vehicle, and the second measurement data are position information of a plurality of ultra-wideband base stations and distance information corresponding to the vehicle respectively by the ultra-wideband base stations;

Determining first position information corresponding to the vehicle at the current moment according to the vehicle speed information measured by the vehicle inertia measuring unit at the current moment, and determining second position information corresponding to the vehicle at the current moment according to the position information of a plurality of ultra-wideband base stations at the current moment and each distance information corresponding to the vehicle by each ultra-wideband base station;

And predicting the position information of the vehicle at the current moment according to a factor graph model based on the first position information of the vehicle at the current moment and the second position information of the vehicle at the current moment to obtain the target position information of the vehicle at the current moment, wherein the factor graph model is constructed based on historical vehicle speed information, historical position information of a plurality of ultra-wideband base stations and distance information corresponding to the historical vehicle respectively by the ultra-wideband base stations.

2. The method of claim 1, wherein the predicting, by a factor graph model, the location information of the vehicle at the current time, further comprises:

Acquiring first measurement data of a vehicle at each historical moment and second measurement data of the vehicle at each historical moment;

constructing a first measurement factor node and a second measurement factor node based on a measurement equation, wherein the first measurement factor node is connected with first measurement data of the vehicle at each historical moment and a corresponding first variable node, and the second measurement factor node is connected with second measurement data of the vehicle at each historical moment and a corresponding second variable node;

The position information of the vehicle at each history moment is used as a fusion variable node, a first state transfer factor node and a second state transfer factor node are constructed based on a state equation, the first state transfer factor node is connected with the first variable node at the history moment before and after, the second state transfer factor node is connected with the second variable node at the history moment before and after, and the fusion variable node is obtained by fusion of the corresponding first state transfer factor node and second state transfer factor node;

And constructing the factor graph model according to the first measurement factor node, the second measurement factor node, the first state transfer factor node, the second state transfer factor node, the first variable node, the second variable node and the fusion variable node.

3. The method according to claim 2, wherein the predicting, by a factor graph model, the position information of the vehicle at the current time based on the first position information of the vehicle at the current time and the second position information of the vehicle at the current time to obtain the target position information of the vehicle at the current time includes:

Calculating first position information corresponding to the current moment through the first measurement factor node, the first state transfer factor node and a vehicle first state quantity at the previous moment to obtain a vehicle first state pre-measurement value corresponding to the current moment and related to the inertia measurement unit;

Calculating second position information corresponding to the current moment through the second measurement factor node, the second state transfer factor node and a vehicle second state quantity at the previous moment to obtain vehicle second state pre-measurement corresponding to the current moment and related to the inertia measurement unit;

and carrying out fusion processing on the basis of the vehicle first state prediction quantity corresponding to the current moment and the vehicle second state prediction quantity corresponding to the current moment and related to the ultra-wideband, so as to obtain the target position information of the vehicle at the current moment.

4. The method according to claim 3, wherein the calculating the first position information corresponding to the current time by the first measurement factor node, the first state transition factor node, and the vehicle first state quantity at the previous time to obtain the vehicle first state prediction value corresponding to the current time with respect to the inertial measurement unit includes:

Predicting the first state quantity of the vehicle at the current moment according to the first state quantity of the vehicle at the previous moment and the first state transfer factor node to obtain a first initial state prediction quantity of the vehicle at the current moment;

performing error calculation based on a vehicle first initial state prediction amount of a vehicle at the current moment and first position information corresponding to the current moment to obtain first loss;

Updating the first measurement factor node according to the first loss to obtain an updated first measurement factor node;

updating the first state transfer factor node based on the first state quantity of the vehicle at the previous moment and the first position information corresponding to the current moment to obtain an updated first state transfer factor node;

and calculating the first position information corresponding to the current moment through the updated first measurement factor node to obtain the vehicle first state prediction quantity corresponding to the current moment and related to the inertia measurement unit.

5. A method according to claim 3, wherein the calculating the second position information corresponding to the current time by the second measurement factor node, the second state transition factor node, and the vehicle second state quantity at the previous time to obtain the vehicle second state prediction value corresponding to the current time with respect to the inertial measurement unit includes:

Predicting the second state quantity of the vehicle at the current moment according to the second state quantity of the vehicle at the previous moment and the second state transfer factor node to obtain a second initial state prediction quantity of the vehicle at the current moment;

performing error calculation based on the predicted value of the second initial state of the vehicle at the current moment and the second position information corresponding to the current moment to obtain second loss;

Updating the second measurement factor node according to the second loss to obtain an updated second measurement factor node;

updating the second state transfer factor node based on the second state quantity of the vehicle at the previous moment and the second position information corresponding to the current moment to obtain an updated second state transfer factor node;

And calculating the second position information corresponding to the current moment through the updated second measurement factor node to obtain the vehicle second state prediction quantity corresponding to the current moment and related to the inertia measurement unit.

6. The method according to claim 1, wherein the vehicle speed information includes an acceleration at the current time and an angular speed at the current time, and the determining the first position information corresponding to the vehicle at the current time based on the vehicle speed information measured by the on-vehicle inertia measurement unit at the current time includes:

integrating the acceleration at the current moment to obtain the displacement of the vehicle;

Integrating the angular speed at the current moment to obtain steering information of the vehicle;

And obtaining first position information corresponding to the vehicle at the current moment based on the displacement of the vehicle and the steering information of the vehicle.

7. The method of claim 2, wherein the state equations comprise inertial measurement unit state equations and ultra-wideband state equations, the constructing first and second state transfer factor nodes based on the state equations comprises:

Carrying out Gaussian distribution on first state quantities of adjacent moments of the vehicle to obtain a first Gaussian distribution result;

Constructing a first state transition factor node based on the first Gaussian distribution result and the inertial measurement unit state equation, wherein the first state transition factor node represents probability distribution of state transition from one moment to the next moment;

carrying out Gaussian distribution on second state quantities of adjacent moments of the vehicle to obtain a second Gaussian distribution result;

And constructing a second state transfer factor node based on the second Gaussian distribution result and the ultra-wide band state equation, wherein the second state transfer factor node represents probability distribution of state transfer from one moment to the next moment.

8. A vehicle positioning device, characterized by comprising:

The acquisition module is used for acquiring first measurement data and second measurement data of a vehicle at the current moment, wherein the first measurement data are vehicle speed information measured by an inertial measurement unit on the vehicle, and the second measurement data are position information of a plurality of ultra-wideband base stations and distance information corresponding to the vehicle respectively;

The calculation module is used for determining first position information corresponding to the vehicle at the current moment according to the vehicle speed information measured by the vehicle-mounted inertia measurement unit at the current moment, and determining second position information corresponding to the vehicle at the current moment according to the position information of a plurality of ultra-wideband base stations at the current moment and the distance information corresponding to the vehicle of each ultra-wideband base station respectively;

The prediction module is used for predicting the position information of the vehicle at the current moment through a factor graph model based on the first position information of the vehicle at the current moment and the second position information of the vehicle at the current moment to obtain the target position information of the vehicle at the current moment, and the factor graph model is constructed based on historical vehicle speed information, historical position information of a plurality of ultra-wideband base stations and distance information corresponding to the historical vehicle respectively by the ultra-wideband base stations.

9. An electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor implements the steps of the method according to any of claims 1 to 7 when the computer program is executed.

10. A readable storage medium storing a computer program, characterized in that the computer program when executed by a processor implements the steps of the method according to any one of claims 1 to 7.