CN117278941B - Vehicle driving auxiliary positioning method and device based on 5G network and data fusion - Google Patents

Abstract

The invention discloses a vehicle driving auxiliary positioning method and device based on 5G network and data fusion, wherein the method comprises the following steps: uploading multisource observation data of a vehicle to be positioned to a cloud platform based on a 5G network, correcting the pose by using a factor graph algorithm, determining a factor graph loss function according to a corrected observation data point set, determining an optimization target according to the factor graph loss function, and solving to obtain a bicycle positioning result; acquiring a multi-target detection result set of the vehicle to be positioned at each moment according to the corrected observation data point set; according to the bicycle positioning result, determining the target state of the vehicle to be positioned, and performing global coordinate system conversion on the multi-target detection result set to obtain a global observation set; and according to the target state and the global observation set, obtaining a corrected target state by using a Gaussian mixture probability hypothesis density filter, and feeding back the corrected target state to the vehicle to be positioned by a 5G network as a positioning result. The invention can realize the vehicle positioning with high precision, high reliability and high real-time performance.

Description

Vehicle driving auxiliary positioning method and device based on 5G network and data fusion

Technical Field

The invention relates to the technical field of vehicles, in particular to a vehicle driving auxiliary positioning method and device based on 5G network and data fusion.

Background

Along with rapid promotion of technological change, intelligent network-connected vehicles can carry advanced devices such as vehicle-mounted sensors, controllers and actuators, and integrate modern communication and network technologies, so that intelligent information exchange and sharing of vehicles, roads, people, clouds and the like are realized, the intelligent network-connected vehicles have the functions of complex environment perception, intelligent decision, cooperative control and the like, and safe, efficient, comfortable and energy-saving running can be realized.

In some driving scenarios, for example, the intelligent network-connected vehicle runs in an airport and can be influenced by weather and the sliding state of an airplane, the prior art generally relies on single-vehicle positioning to realize the positioning of the intelligent network-connected vehicle, for example, the intelligent network-connected vehicle is positioned through an inertial navigation system, the intelligent network-connected vehicle is easy to receive signals due to external interference, the positioning error is gradually increased along with the accumulation of time, and the high-precision and high-reliability vehicle positioning is difficult to realize by only relying on single-vehicle positioning.

Disclosure of Invention

The invention provides a vehicle driving auxiliary positioning method and device based on 5G network and data fusion, which can realize high-precision, high-reliability and high-real-time vehicle positioning by uploading multi-source observation data of a vehicle to a cloud platform and utilizing a Gaussian mixture probability hypothesis density filter to carry out multi-target cooperative positioning on the vehicle.

In order to solve the technical problems, a first aspect of the embodiment of the present invention provides a vehicle driving assistance positioning method based on 5G network and data fusion, including the following steps:

Uploading multisource observation data of a vehicle to be positioned to a cloud platform based on a 5G network, and correcting the pose of the multisource observation data by using a factor graph algorithm to obtain a corrected observation data point set;

determining a factor graph loss function according to the corrected observation data point set, determining an optimization target according to the factor graph loss function, and solving the optimization target by utilizing a nonlinear optimization algorithm to obtain a bicycle positioning result;

acquiring a multi-target detection result set of the vehicle to be positioned at each moment according to the corrected observation data point set;

Determining the target state of the vehicle to be positioned according to the bicycle positioning result, and performing global coordinate system conversion on the multi-target detection result set to obtain a global observation set;

and according to the target state and the global observation set, obtaining a corrected target state of the vehicle to be positioned by using a Gaussian mixture probability hypothesis density filter, and feeding back the corrected target state to the vehicle to be positioned as a positioning result by a 5G network.

Preferably, the multi-source observation data at least comprises Beidou observation data, inertial sensor observation data, laser radar observation data and camera observation data.

As a preferred scheme, the pose correction is performed on the multi-source observation data by using a factor graph algorithm, and the method specifically comprises the following steps:

constructing a residual error item of the node pose and the Beidou observation data according to the Beidou observation data in the multi-source observation data;

adding disturbance to the node pose in the residual error term and solving to obtain a Jacobian matrix of the residual error term relative to the disturbance of the node pose;

correcting the Beidou observation data by utilizing the Jacobian matrix;

the residual term of the ith node is as follows: T _i represents the pose to be optimized of the ith node, Z _i represents the Beidou observation data of the ith node, and ζ _zi and ζ _i respectively represent the pose to be optimized and the corresponding lie algebra of the Beidou observation data; the residual terms of the disturbance added to the node pose of the ith node are as follows: Δζ _i represents a disturbance term of a lie algebra corresponding to pose to be optimized and Beidou observation data, and G _i represents a jacobian matrix of disturbance of a residual term relative to the pose of the node.

As a preferred solution, the method for correcting the pose of the multi-source observation data by using a factor graph algorithm specifically further includes the following steps:

Constructing residual items of node pose and the inertia sensor observation data, the laser radar observation data and the camera observation data according to the inertia sensor observation data, the laser radar observation data and the camera observation data in the multi-source observation data;

adding disturbance to the node pose in the residual error term and solving to obtain a Jacobian matrix of the residual error term relative to the disturbance of the node pose;

Correcting the inertial sensor observation data, the laser radar observation data and the camera observation data by utilizing the Jacobian matrix;

The residual error items between the ith node and the jth node are as follows: T _i and T _j respectively represent pose to be optimized of an ith node and a jth node, Z _ij represents multi-source observation data of the pose of the ith node and the jth node, and ζ _i、ξ_j and ζ _ij respectively represent lie algebra corresponding to the pose to be optimized and the multi-source observation data; the residual error term of adding disturbance to the node position between the ith node and the jth node is as follows: /(I) Δζ _i and Δζ _j represent disturbance terms of the lie algebra corresponding to the pose to be optimized and the multi-source observed data, and G _i and G _j represent jacobian matrices of the residual terms relative to the node pose disturbance.

As a preferred solution, the optimization objective is:

Wherein X ^* represents a point set of an optimal factor graph, X represents a point set of a factor graph, and F (X) represents the factor graph loss function:

e _i and e _ij represent residual terms corresponding to a unitary edge and a binary edge, respectively, Ω _i represents covariance of a noise model corresponding to a unitary edge or a binary edge, Representing the 1/2 matrix of omega _i.

As a preferred scheme, the method uses a nonlinear optimization algorithm to solve the optimization target to obtain a bicycle positioning result, specifically:

and solving the optimization target by using a Levenberg-Marquardt iteration method to obtain the bicycle positioning result.

As a preferred solution, the multi-target detection result set is:

Wherein M represents an mth object detected by the vehicle to be positioned, M represents the total number of the objects detected by the vehicle to be positioned, n represents the number of the vehicle to be positioned, T _n,m,k represents the target detection result of the vehicle to be positioned on the mth object at the kth moment, Error indicating the target detection result, x _m,k indicates the state of the mth object, x _n,k indicates the state of the vehicle to be positioned, and H ^(V2T) is:

as a preferable mode, the target state of the vehicle to be positioned is:

Tr_n,k＝{n,x_n,k,P_n,k}；

wherein P _n,k represents a state covariance matrix of the vehicle to be positioned;

the global observation set is:

Wherein, Representing a global observation of the mth object by the vehicle to be localized,C represents the number of the cloud platform, H represents the observation sequence number, H represents the total amount of observation,/>Representing an error between the state of the vehicle to be positioned and a posterior state.

As a preferred solution, the obtaining, according to the target state and the global observation set, the corrected target state of the vehicle to be positioned by using a gaussian mixture probability hypothesis density filter specifically includes the following steps:

acquiring posterior Gaussian mixture probability hypothesis density of the vehicle to be positioned at each moment by using a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set; the posterior Gaussian mixture probability hypothesis density comprises a plurality of Gaussian components, wherein the Gaussian components comprise a vehicle number, the possibility that the state of the vehicle to be positioned generates global observation of any one object at the current moment, a posterior state obtained by correcting the state of the vehicle to be positioned by using the global observation of any one object and a state covariance matrix corresponding to the posterior state;

Deleting Gaussian components with the probability smaller than a probability threshold in the postulated density of the posterior Gaussian mixture probability based on a preset probability threshold;

and merging the target Gaussian components with the same vehicle number as the vehicle to be positioned according to the plurality of Gaussian components after the deletion treatment to obtain a merging posterior state and a merging state covariance matrix, and taking the merging posterior state and the merging state covariance matrix as correction target states of the vehicle to be positioned.

A second aspect of the embodiment of the present invention provides a vehicle driving assistance positioning device based on 5G network and data fusion, including:

The pose correction module is used for uploading multisource observation data of a vehicle to be positioned to the cloud platform by the 5G network, and carrying out pose correction on the multisource observation data by utilizing a factor graph algorithm to obtain a corrected observation data point set;

The bicycle positioning module is used for determining a factor graph loss function according to the corrected observation data point set, determining an optimization target according to the factor graph loss function, and solving the optimization target by utilizing a nonlinear optimization algorithm to obtain a bicycle positioning result;

the multi-target detection result set acquisition module is used for acquiring a multi-target detection result set of the vehicle to be positioned at each moment according to the corrected observation data point set;

The target state and global observation set acquisition module is used for determining the target state of the vehicle to be positioned according to the bicycle positioning result, and performing global coordinate system conversion on the multi-target detection result set to obtain a global observation set;

And the positioning result acquisition module is used for acquiring a corrected target state of the vehicle to be positioned by utilizing a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set, and feeding back the corrected target state to the vehicle to be positioned as a positioning result through a 5G network.

Compared with the prior art, the method and the device have the beneficial effects that the vehicle positioning with high accuracy, high reliability and high real-time performance can be realized by uploading the multi-source observation data of the vehicle to the cloud platform and utilizing the Gaussian mixture probability hypothesis density filter to carry out multi-target cooperative positioning on the vehicle.

Drawings

FIG. 1 is a flow chart of a vehicle driving assistance localization real-time method based on 5G network and data fusion in an embodiment of the invention;

FIG. 2 is a schematic diagram of a multi-vehicle information interaction architecture in an embodiment of the present invention;

FIG. 3 is a factor graph of a unary edge in an embodiment of the invention;

FIG. 4 is a factor graph of binary edges in an embodiment of the invention;

FIG. 5 is a schematic diagram of an implementation of a vehicle driving assistance localization real-time method based on 5G network and data fusion in an embodiment of the invention;

fig. 6 is a schematic structural diagram of a vehicle driving assistance positioning device based on 5G network and data fusion in an embodiment of the invention.

Detailed Description

The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

Referring to fig. 1, a first aspect of the embodiment of the present invention provides a vehicle driving assistance positioning method based on 5G network and data fusion, including steps S1 to S5 as follows:

step S1, uploading multisource observation data of a vehicle to be positioned to a cloud platform based on a 5G network, and carrying out pose correction on the multisource observation data by utilizing a factor graph algorithm to obtain a corrected observation data point set;

S2, determining a factor graph loss function according to the corrected observation data point set, determining an optimization target according to the factor graph loss function, and solving the optimization target by utilizing a nonlinear optimization algorithm to obtain a bicycle positioning result;

Step S3, a multi-target detection result set of the vehicle to be positioned at each moment is obtained according to the corrected observation data point set;

s4, determining the target state of the vehicle to be positioned according to the bicycle positioning result, and performing global coordinate system conversion on the multi-target detection result set to obtain a global observation set;

And S5, according to the target state and the global observation set, obtaining a corrected target state of the vehicle to be positioned by using a Gaussian mixture probability hypothesis density filter, and feeding back the corrected target state to the vehicle to be positioned as a positioning result through a 5G network.

In this embodiment, the vehicle to be positioned uploads the multisource observation data to the cloud platform based on the 5G mobile communication network, the 5G mobile communication fuses key technologies such as a large-scale antenna array, ultra-dense networking and millimeter waves, the delay average value is reduced to be within 10ms, the requirements of the internet of vehicles on low delay, large bandwidth and high speed and the like can be met, and the 5G supports the slicing network, so that slicing management can be implemented on data with different priorities in transmission. Fig. 2 is a schematic diagram of a multi-vehicle information interaction architecture in an embodiment of the present invention, where a single vehicle uploads its multi-source observation data to a cloud platform through a 5G mobile communication network, and V2V communication is performed between vehicles.

Further, in the embodiment, the pose correction is performed on the multi-source observed data by using a factor graph algorithm, and the node pose is adjusted so as to be closer to the observed result.

Preferably, the multi-source observation data at least comprises Beidou observation data, inertial sensor observation data, laser radar observation data and camera observation data.

As a preferred scheme, the pose correction is performed on the multi-source observation data by using a factor graph algorithm, and the method specifically comprises the following steps:

constructing a residual error item of the node pose and the Beidou observation data according to the Beidou observation data in the multi-source observation data;

adding disturbance to the node pose in the residual error term and solving to obtain a Jacobian matrix of the residual error term relative to the disturbance of the node pose;

correcting the Beidou observation data by utilizing the Jacobian matrix;

the residual term of the ith node is as follows: T _i represents the pose to be optimized of the ith node, Z _i represents the Beidou observation data of the ith node, and ζ _zi and ζ _i respectively represent the pose to be optimized and the corresponding lie algebra of the Beidou observation data; the residual terms of the disturbance added to the node pose of the ith node are as follows: Δζ _i represents a disturbance term of a lie algebra corresponding to pose to be optimized and Beidou observation data, and G _i represents a jacobian matrix of disturbance of a residual term relative to the pose of the node.

Specifically, the edges, i.e., unitary edges, of the Beidou observation data associated with only one node in the factor graph represent direct observations of a certain node pose. Referring to fig. 3, a factor graph of a unitary edge is provided for an embodiment of the present invention.

Firstly, constructing residual items of node pose and Beidou observation data, wherein the residual items of the ith node are as follows:

Wherein, T _i represents the pose to be optimized of the ith node, Z _i represents the Beidou observation data of the ith node, ζ _zi and ζ _i respectively represent the lie algebra corresponding to the pose to be optimized and the Beidou observation data, (. Cndot.) ^∧ represents the antisymmetric matrix corresponding to the vector, and (-) ^∨ represents the mapping of the antisymmetric matrix into a three-dimensional vector. Ideally, e _i should be 0, but due to the presence of observation noise, e _i is greater than 0, so by adjusting the pose, e _i is made as small as possible.

Then, adding disturbance to the node pose in the residual error term to calculate a corresponding Jacobian matrix, wherein the residual error term of the disturbance to the node pose of the ith node is as follows:

Using BCH formulas and applying the concomitant properties of lie algebra, the residual terms can be reduced to:

Wherein Δζ _i represents a disturbance term of a lie algebra corresponding to pose to be optimized and Beidou observation data, and G _i represents a jacobian matrix of disturbance of a residual term relative to the pose of the node:

I is an identity matrix, phi _e is the lie algebra of the residual term, and rho _e is the norm of the residual term.

As a preferred solution, the method for correcting the pose of the multi-source observation data by using a factor graph algorithm specifically further includes the following steps:

Constructing residual items of node pose and the inertia sensor observation data, the laser radar observation data and the camera observation data according to the inertia sensor observation data, the laser radar observation data and the camera observation data in the multi-source observation data;

adding disturbance to the node pose in the residual error term and solving to obtain a Jacobian matrix of the residual error term relative to the disturbance of the node pose;

Correcting the inertial sensor observation data, the laser radar observation data and the camera observation data by utilizing the Jacobian matrix;

The residual error items between the ith node and the jth node are as follows: T _i and T _j respectively represent pose to be optimized of an ith node and a jth node, Z _jj represents multi-source observation data of the pose of the ith node and the jth node, and ζ _i、ξ_j and ζ _ij respectively represent lie algebra corresponding to the pose to be optimized and the multi-source observation data; the residual error term of adding disturbance to the node position between the ith node and the jth node is as follows: /(I) Δζ _i and Δζ _j represent disturbance terms of the lie algebra corresponding to the pose to be optimized and the multi-source observed data, and G _i and G _j represent jacobian matrices of the residual terms relative to the node pose disturbance.

Specifically, the IMU pre-integration, the BA of the camera (Bundle Adjustment, beam adjustment method) and the point cloud registration algorithm of the lidar have associated edges, i.e., binary edges, in the factor graph and at both nodes, representing the observation of the relative pose of some two nodes. Referring to fig. 4, a factor graph of binary edges is provided for an embodiment of the present invention.

Firstly, constructing residual items of node pose, inertial sensor observation data, laser radar observation data and camera observation data, wherein the residual items between an ith node and a jth node are as follows:

Wherein, T _i and T _j respectively represent pose to be optimized of the ith node and the jth node, Z _ij represents multi-source observation data of the pose of the ith node and the jth node, and ζ _i、ξ_j and ζ _ij respectively represent lie algebra corresponding to the pose to be optimized and the multi-source observation data. Ideally, e _ij should be 0, but due to the presence of observation noise, e _ij is greater than 0, so by adjusting the pose, e _ij is made as small as possible.

Then, adding disturbance to the node pose in the residual terms to calculate a corresponding jacobian matrix, wherein the residual terms of the disturbance to the node pose between the ith node and the jth node are as follows:

Using BCH formulas and applying the concomitant properties of lie algebra, the residual terms can be reduced to:

Wherein Δζ _i and Δζ _j represent disturbance terms of the lie algebra corresponding to the pose to be optimized and the multi-source observed data, and G _i and G _j represent jacobian matrices of residual terms relative to the node pose disturbance:

As a preferred solution, the optimization objective is:

Wherein X ^* represents a point set of an optimal factor graph, X represents a point set of a factor graph, and F (X) represents the factor graph loss function:

e _i and e _ij represent residual terms corresponding to a unitary edge and a binary edge, respectively, Ω _i represents covariance of a noise model corresponding to a unitary edge or a binary edge, Representing the 1/2 matrix of omega _i.

Specifically, the point set of the factor graph is { x _i }, the unary edge set is { e _i }, and the binary edge set is { e _ij }. Thus, the overall loss function of the factor graph is:

wherein e _i and e _ij represent residual terms corresponding to the unitary edge and the binary edge, respectively, and Ω _i represents covariance of the noise model corresponding to the unitary edge or the binary edge.

In order to use nonlinear optimization means, the least squares problem of converting the mahalanobis distance of the residual to a 2-norm is required:

Therefore, the loss function of the whole factor graph is converted as follows:

The optimization targets are as follows:

As a preferred scheme, the method uses a nonlinear optimization algorithm to solve the optimization target to obtain a bicycle positioning result, specifically:

and solving the optimization target by using a Levenberg-Marquardt iteration method to obtain the bicycle positioning result.

Specifically, the embodiment of the invention adopts a Levenberg-Marquardt iteration method to solve, and the optimization target is converted into:

Using the levenberg-marquardt iterative method, the delta equation is:

(J(X)(J(X))^T+λI)Δx＝-J(X)F(X)；

wherein e is a constant, delta is an increment form, and the algorithm steps are as follows:

(1) Giving an initial value x ₀ and an initial radius mu;

(2) At the kth iteration, computing the jacobian matrix J (X _k) and the error F (X _k);

(3) Solving delta X _k in an incremental equation;

(4) Calculating ρ:

(5) If it is Then μ=2μ is set;

(6) If it is Then μ=0.5 μ is set;

(7) If ρ is greater than a preset threshold, let X _k+1＝X_k+ΔX_k go through step (2); otherwise, the algorithm is terminated.

As a preferred solution, the multi-target detection result set is:

Wherein M represents an mth object detected by the vehicle to be positioned, M represents the total number of the objects detected by the vehicle to be positioned, n represents the number of the vehicle to be positioned, T _n,m,k represents the target detection result of the vehicle to be positioned on the mth object at the kth moment, Error indicating the target detection result, x _m,k indicates the state of the mth object, x _n,k indicates the state of the vehicle to be positioned, and H ^(V2T) is:

as a preferable mode, the target state of the vehicle to be positioned is:

Tr_n,k＝{n,x_n,k,P_n,k}；

wherein P _n,k represents a state covariance matrix of the vehicle to be positioned;

the global observation set is:

Wherein, Representing a global observation of the mth object by the vehicle to be localized,C represents the number of the cloud platform, H represents the observation sequence number, H represents the total amount of observation,/>Representing an error between the state of the vehicle to be positioned and a posterior state.

Specifically, at the kth time, the target detection result of the mth object by the vehicle to be positioned may be expressed as:

Wherein, Representing the error of the target detection result and assuming/>Is measurement noise belonging to Gaussian white noise, i.e./> Is the relative vector of the vehicle, i.e., the relative position and relative velocity between the vehicle and surrounding objects, the superscript V2T is an abbreviation for vehicle-to-target (Vehicle to Target), and R is the covariance matrix.

The above equation shows that if measurement noise is not considered, the host vehicle observation T _n,m,k is equal to the difference (without acceleration information) between the state x _m,k obtained by the sensor on the object m and the state x _n,k obtained by the sensor on the vehicle to be positioned.

In summary, at the kth time, the set of multi-target detection results of the vehicle to be positioned is:

Wherein M represents an mth object detected by the vehicle to be positioned, M represents the total number of the objects detected by the vehicle to be positioned, and n represents the number of the vehicle to be positioned. Because the total number of the objects detected by different intelligent network vehicles is different, the objects are expressed in the form of M (n) Abbreviated as Γ _nk＝{T_n,m,k.

As a preferable mode, the target state of the vehicle to be positioned is:

Tr_n,k＝{n,x_n,k,P_n,k}；

wherein P _n,k represents a state covariance matrix of the vehicle to be positioned;

the global observation set is:

Wherein, Representing a global observation of the mth object by the vehicle to be localized,C represents the number of the cloud platform, H represents the observation sequence number, H represents the total amount of observation,/>Representing an error between the state of the vehicle to be positioned and a posterior state.

Specifically, a bicycle positioning result (including a number, a state and a state covariance matrix) of the vehicle to be positioned is extracted, and a target state is obtained, namely:

Tr_n,k＝L_n,k＝{n,x_n,k,P_n,k}；

Each label state corresponds to an intelligent network connection vehicle, and the corresponding relation can be obtained clearly through the intelligent network connection vehicle number. The cloud platform aims at searching for the observation of the target (namely the intelligent network-connected vehicle) at the cloud end, and further, the state of the intelligent network-connected vehicle is estimated by using the found observation, so that a higher-precision estimation result is obtained.

Since the vehicle relative vector Γ _nk＝{T_n,m,k is in the on-vehicle coordinate system, the relative vector cannot be directly used to correct the target state, and it is necessary to convert the vehicle relative vector Γ _nk＝{T_n,m,k into the global coordinate system. From the multi-target detection result set, the state of the mth object can be expressed as:

combining the above formulas can obtain:

Order the Representing global observations of the mth object (containing only position and speed information) by the vehicle to be positioned, h representing the observation sequence number, which is the sequence number of all global observations ordered according to the generation time,Representing the error between the state of the vehicle to be positioned and the posterior state, i.e./>Thus, the vehicle relative vector of the vehicle to be localized is converted into a global set of observations:

The task of the cloud positioning server is to estimate the target state from the global observations Z _k, which is consistent with the "multi-target tracking problem: the definition of estimating the states of multiple targets simultaneously using sensor observations.

As a preferred solution, the obtaining, according to the target state and the global observation set, the corrected target state of the vehicle to be positioned by using a gaussian mixture probability hypothesis density filter specifically includes the following steps:

acquiring posterior Gaussian mixture probability hypothesis density of the vehicle to be positioned at each moment by using a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set; the posterior Gaussian mixture probability hypothesis density comprises a plurality of Gaussian components, wherein the Gaussian components comprise a vehicle number, the possibility that the state of the vehicle to be positioned generates global observation of any one object at the current moment, a posterior state obtained by correcting the state of the vehicle to be positioned by using the global observation of any one object and a state covariance matrix corresponding to the posterior state;

Deleting Gaussian components with the probability smaller than a probability threshold in the postulated density of the posterior Gaussian mixture probability based on a preset probability threshold;

and merging the target Gaussian components with the same vehicle number as the vehicle to be positioned according to the plurality of Gaussian components after the deletion treatment to obtain a merging posterior state and a merging state covariance matrix, and taking the merging posterior state and the merging state covariance matrix as correction target states of the vehicle to be positioned.

Specifically, a multi-target tracking method under a random set framework, in which both targets and observations are treated as random finite sets, and then the number of targets and the states of the targets are estimated simultaneously using a multi-target bayesian filtering technique.

In an intelligent network-connected vehicle, the estimated values of the number and the state of targets are known, and the prediction steps in the cloud multi-target tracking process can be simplified as follows:

Given the k-moment Gaussian mixture probability density prediction result of the formula target state and given the k-moment M= |z _k |observations z _k＝{z_1,k,…,z_M,k }, the posterior mixture probability assumption density form at k-moment is:

Wherein,

q_m,n,k(z_m)＝N(z_m;η_n,k|k-1,S_n,k)；

η_n,k＝H_km_n,k|k-1；

m_m,n,k＝m_n,k|k-1+K_n,k(z_m-η_n,k)；

P_m,n,k＝P_n,k|k-1-K_n,kH_kP_n,k|k-1；

The (1-p _D) in the posterior Gaussian mixture probability hypothesis density D _k(x_k) represents that all sensors miss targets, and according to the cloud positioning system model, the Gaussian components in the prediction step are all states of real targets, and the condition that the sensors miss targets does not exist, so that the (1-p _D)D_k|k-1(x_k|k-1) part is not needed to be calculated; Representing the correction of the observed z _m,k to the gaussian component N (x _k|;m_n,k|k-1,P_n,k|k-1) in the probability hypothesis density D _k|k-1(x_k|k-1), each observed z _m,k produces N correction results; q _m,n,k(z_m) is the approximation between observation z _m and gaussian component N (x _k|;m_n,k|k-1,P_n,k|k-1), i.e. the probability of generating observation z _m from target N (x _k|;m_n,k|k-1,P_n,k|k-1); k _k(z_m) is the probability that observation z _m belongs to clutter.

The postulated density of the posterior Gaussian mixture probability at the kth moment comprises J _k = M x N Gaussian vectorsT _j,k in these gaussian vectors represents the label of the vector, i.e. the vehicle code; omega _j,k represents the likelihood of generating a global observation z _m,k of any one object from the state x _n,k of the vehicle to be localized; m _j,k represents a posterior state obtained by correcting the state x _n,k of the vehicle to be positioned using the global observation z _m,k of any one object; p _j,k represents the state covariance matrix corresponding to the posterior state m _j,k. Based on a preset likelihood threshold, gaussian components in the postulated density of the posterior Gaussian mixture probability, which have the likelihood of generating global observations z _m,k of any object by the state x _n,k of the vehicle to be positioned, are deleted. Illustratively, assume that the likelihood threshold is: ω _p =0.5, then the gaussian component whose probability of generating the global observation z _m,k of any object from the state x _n,k of the vehicle to be localized is less than 0.5 is deleted.

For several gaussian components after the deletion process, the gaussian components with the same label represent the correction results for the same vehicle, and these correction results need to be combined to obtain the final correction target state. For L gaussian components of tag t _n,k =nThe merging step is performed according to the following formula: /(I)

Where e _l,n＝m_n,k-m_l,k, subscript l represents the sequence number of the combined gaussian component. The final correction result of the target state with the label of n is { m _n,k,P_n,k }, and the result is fed back to the vehicle to be positioned as a positioning result.

As shown in fig. 5, in the embodiment of the invention, the special vehicle n/m uploads the multi-source sensor data to the cloud platform, the cloud platform transmits the received multi-source sensor data to the mobile edge computing module through data interaction, and the mobile edge computing module respectively performs single vehicle multi-source data pose correction, multi-vehicle target detection, target state and global state observation and multi-target fusion positioning, finally feeds positioning information back to the special vehicle n/m, and meanwhile, the special vehicle n and the special vehicle m interact positioning information.

According to the vehicle driving auxiliary positioning method based on the 5G network and the data fusion, the vehicle multi-source observation data are uploaded to the cloud platform, and the Gaussian mixture probability hypothesis density filter is utilized to perform multi-target cooperative positioning on the vehicle, so that the vehicle positioning with high accuracy, high reliability and high instantaneity can be realized.

Referring to fig. 6, a second aspect of the embodiment of the present invention provides a vehicle driving assistance positioning device based on 5G network and data fusion, including:

the pose correction module 601 is configured to upload multi-source observation data of a vehicle to be positioned to a cloud platform based on a 5G network, and perform pose correction on the multi-source observation data by using a factor graph algorithm to obtain a corrected observation data point set;

the bicycle positioning module 602 is configured to determine a factor graph loss function according to the corrected observation data point set, determine an optimization target according to the factor graph loss function, and solve the optimization target by using a nonlinear optimization algorithm to obtain a bicycle positioning result;

A multi-target detection result set acquisition module 603, configured to obtain a multi-target detection result set of the vehicle to be positioned at each moment according to the corrected observation data point set;

The target state and global observation set obtaining module 604 is configured to determine a target state of the vehicle to be positioned according to the bicycle positioning result, and perform global coordinate system conversion on the multi-target detection result set to obtain a global observation set;

The positioning result obtaining module 605 is configured to obtain, according to the target state and the global observation set, a corrected target state of the vehicle to be positioned by using a gaussian mixture probability hypothesis density filter, and feed back the corrected target state as a positioning result to the vehicle to be positioned by a 5G network.

It should be noted that, the vehicle driving assistance positioning device based on 5G network and data fusion provided by the embodiment of the present invention can implement all the processes of the vehicle driving assistance positioning method based on 5G network and data fusion described in any of the above embodiments, and the functions and the implemented technical effects of each module in the device are respectively the same as those of the vehicle driving assistance positioning method based on 5G network and data fusion described in the above embodiments, and are not repeated herein.

While the foregoing is directed to the preferred embodiments of the present invention, it will be appreciated by those skilled in the art that changes and modifications may be made without departing from the principles of the invention, such changes and modifications are also intended to be within the scope of the invention.

Claims (8)

1. A vehicle driving auxiliary positioning method based on 5G network and data fusion is characterized by comprising the following steps:

Uploading multisource observation data of a vehicle to be positioned to a cloud platform based on a 5G network, and correcting the pose of the multisource observation data by using a factor graph algorithm to obtain a corrected observation data point set;

determining a factor graph loss function according to the corrected observation data point set, determining an optimization target according to the factor graph loss function, and solving the optimization target by utilizing a nonlinear optimization algorithm to obtain a bicycle positioning result;

acquiring a multi-target detection result set of the vehicle to be positioned at each moment according to the corrected observation data point set;

Determining the target state of the vehicle to be positioned according to the bicycle positioning result, and performing global coordinate system conversion on the multi-target detection result set to obtain a global observation set;

according to the target state and the global observation set, a Gaussian mixture probability hypothesis density filter is utilized to obtain a correction target state of the vehicle to be positioned, and the correction target state is used as a positioning result and fed back to the vehicle to be positioned by a 5G network;

wherein, the optimization objective is:

Wherein X ^* represents a point set of an optimal factor graph, X represents a point set of a factor graph, and F (X) represents the factor graph loss function:

e _i and e _ij represent residual terms corresponding to a unitary edge and a binary edge, respectively, Ω _i represents covariance of a noise model corresponding to a unitary edge or a binary edge, A 1/2 matrix representing Ω _i;

The method for obtaining the corrected target state of the vehicle to be positioned by using a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set specifically comprises the following steps:

Acquiring posterior Gaussian mixture probability hypothesis density of the vehicle to be positioned at each moment by using a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set; the posterior Gaussian mixture probability hypothesis density comprises a plurality of Gaussian components, wherein the Gaussian components comprise a vehicle number, a posterior state, a state covariance matrix corresponding to the posterior state and the possibility that the state of the vehicle to be positioned generates global observation on any object at the current moment; the state of the vehicle to be positioned is obtained from the target state, and the posterior state is obtained by correcting the state of the vehicle to be positioned by using global observation of any one object;

Deleting Gaussian components with the probability smaller than a probability threshold in the postulated density of the posterior Gaussian mixture probability based on a preset probability threshold;

and merging the target Gaussian components with the same vehicle number as the vehicle to be positioned according to the plurality of Gaussian components after the deletion treatment to obtain a merging posterior state and a merging state covariance matrix, and taking the merging posterior state and the merging state covariance matrix as correction target states of the vehicle to be positioned.

2. The vehicle driving assistance localization real-time method based on 5G network and data fusion of claim 1, wherein said multi-source observations comprise at least beidou observations, inertial sensor observations, lidar observations and camera observations.

3. The vehicle driving assistance positioning method based on 5G network and data fusion according to claim 2, wherein the posture correction of the multi-source observation data is performed by using a factor graph algorithm, and specifically comprises the following steps:

constructing a residual error item of the node pose and the Beidou observation data according to the Beidou observation data in the multi-source observation data;

adding disturbance to the node pose in the residual error term and solving to obtain a Jacobian matrix of the residual error term relative to the disturbance of the node pose;

correcting the Beidou observation data by utilizing the Jacobian matrix;

the residual term of the ith node is as follows: T _i represents the pose to be optimized of the ith node, Z _i represents the Beidou observation data of the ith node, and ζ _zi and ζ _i respectively represent the pose to be optimized and the corresponding lie algebra of the Beidou observation data; the residual terms of the disturbance added to the node pose of the ith node are as follows: Δζ _i represents a disturbance term of a lie algebra corresponding to pose to be optimized and Beidou observation data, and G _i represents a jacobian matrix of disturbance of a residual term relative to the pose of the node.

4. The vehicle driving assistance positioning method based on 5G network and data fusion according to claim 2, wherein the posture correction of the multi-source observation data is performed by using a factor graph algorithm, and the method specifically further comprises the following steps:

Constructing residual items of node pose and the inertia sensor observation data, the laser radar observation data and the camera observation data according to the inertia sensor observation data, the laser radar observation data and the camera observation data in the multi-source observation data;

adding disturbance to the node pose in the residual error term and solving to obtain a Jacobian matrix of the residual error term relative to the disturbance of the node pose;

Correcting the inertial sensor observation data, the laser radar observation data and the camera observation data by utilizing the Jacobian matrix;

The residual error items between the ith node and the jth node are as follows: T _i and T _j respectively represent pose to be optimized of an ith node and a jth node, Z _ij represents multi-source observation data of the pose of the ith node and the jth node, and ζ _i、ξ_j and ζ _ij respectively represent lie algebra corresponding to the pose to be optimized and the multi-source observation data; the residual error term of adding disturbance to the node position between the ith node and the jth node is as follows: /(I) Δζ _i and Δζ _j represent disturbance terms of the lie algebra corresponding to the pose to be optimized and the multi-source observed data, and G _i and G _j represent jacobian matrices of the residual terms relative to the node pose disturbance.

5. The vehicle driving assistance positioning method based on 5G network and data fusion according to claim 1, wherein the method is characterized in that the optimization target is solved by using a nonlinear optimization algorithm to obtain a bicycle positioning result, and specifically comprises the following steps:

and solving the optimization target by using a Levenberg-Marquardt iteration method to obtain the bicycle positioning result.

6. The vehicle driving assistance positioning method based on 5G network and data fusion of claim 1, wherein the multi-target detection result set is:

Wherein M represents an mth object detected by the vehicle to be positioned, M represents the total number of the objects detected by the vehicle to be positioned, n represents the number of the vehicle to be positioned, T _n,m,k represents the target detection result of the vehicle to be positioned on the mth object at the kth moment, Error indicating the target detection result, x _m,k indicates the state of the mth object, x _n,k indicates the state of the vehicle to be positioned, and H ^(V2T) is:

7. The vehicle driving assistance positioning method based on 5G network and data fusion according to claim 6, wherein the target state of the vehicle to be positioned is:

Tr_n,k＝{n,x_n,k,P_n,k}；

wherein P _n,k represents a state covariance matrix of the vehicle to be positioned;

the global observation set is:

Wherein, Representing a global observation of the mth object by the vehicle to be localized,C represents the number of the cloud platform, H represents the observation sequence number, H represents the total amount of observation,/>Representing an error between the state of the vehicle to be positioned and a posterior state.

8. A vehicle driving assistance-localization real-time device based on 5G network and data fusion, comprising:

The pose correction module is used for uploading multisource observation data of a vehicle to be positioned to the cloud platform based on a 5G network, and carrying out pose correction on the multisource observation data by utilizing a factor graph algorithm to obtain a corrected observation data point set;

The bicycle positioning module is used for determining a factor graph loss function according to the corrected observation data point set, determining an optimization target according to the factor graph loss function, and solving the optimization target by utilizing a nonlinear optimization algorithm to obtain a bicycle positioning result;

the multi-target detection result set acquisition module is used for acquiring a multi-target detection result set of the vehicle to be positioned at each moment according to the corrected observation data point set;

The target state and global observation set acquisition module is used for determining the target state of the vehicle to be positioned according to the bicycle positioning result, and performing global coordinate system conversion on the multi-target detection result set to obtain a global observation set;

The positioning result acquisition module is used for acquiring a corrected target state of the vehicle to be positioned by utilizing a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set, and feeding back the corrected target state to the vehicle to be positioned as a positioning result through a 5G network;

wherein, the optimization objective is:

Wherein X ^* represents a point set of an optimal factor graph, X represents a point set of a factor graph, and F (X) represents the factor graph loss function:

e _i and e _ij represent residual terms corresponding to a unitary edge and a binary edge, respectively, Ω _i represents covariance of a noise model corresponding to a unitary edge or a binary edge, A 1/2 matrix representing Ω _i;

the positioning result obtaining module is configured to obtain, according to the target state and the global observation set, a corrected target state of the vehicle to be positioned by using a gaussian mixture probability hypothesis density filter, and specifically includes:

Acquiring posterior Gaussian mixture probability hypothesis density of the vehicle to be positioned at each moment by using a Gaussian mixture probability hypothesis density filter according to the target state and the global observation set; the posterior Gaussian mixture probability hypothesis density comprises a plurality of Gaussian components, wherein the Gaussian components comprise a vehicle number, a posterior state, a state covariance matrix corresponding to the posterior state and the possibility that the state of the vehicle to be positioned generates global observation on any object at the current moment; the state of the vehicle to be positioned is obtained from the target state, and the posterior state is obtained by correcting the state of the vehicle to be positioned by using global observation of any one object;

Deleting Gaussian components with the probability smaller than a probability threshold in the postulated density of the posterior Gaussian mixture probability based on a preset probability threshold;

and merging the target Gaussian components with the same vehicle number as the vehicle to be positioned according to the plurality of Gaussian components after the deletion treatment to obtain a merging posterior state and a merging state covariance matrix, and taking the merging posterior state and the merging state covariance matrix as correction target states of the vehicle to be positioned.