Autopilot Positioning Method, Device, Vehicle and Storage Medium for a Vehicle

Abstract

The present application relates to an autonomous driving positioning method, device, vehicle, and storage medium for a vehicle. Among them, the method includes: obtaining a 4D millimeter-wave radar tensor of at least one target; calculating the actual vehicle speed using the 4D millimeter-wave radar tensor; constructing a pose transformation optimization equation based on the tensor error between adjacent frames, and solving the pose transformation optimization equation to obtain a prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor; inputting the actual vehicle speed and the prior pose transformation of the current frame into a pre-constructed extended Kalman filter to obtain an observed pose transformation of the vehicle, and obtaining a positioning result of the vehicle based on the observed pose transformation. Thus, the technical problem in the related art that it is difficult to make full use of the original information of the 4D millimeter-wave radar, and the positioning effect is relatively unstable with large errors is solved.

Description

Technical Field

[0001] The present application relates to the field of radar positioning technology, and in particular to a method, device, vehicle and storage medium for autonomous driving positioning of a vehicle. Background Art

[0002] In the related technologies, the positioning methods based on 4D millimeter wave radar are all implemented based on the point cloud information of 4D millimeter wave radar, which mainly include the following two categories:

[0003] One type is based on the Doppler effect speed of point cloud targets. The vehicle's current speed is calculated based on the least squares method, and then the vehicle movement between two frames is calculated in combination with the timestamp. After accumulation, real-time local positioning is completed. This type of method ignores the perceived target position and only locates based on Doppler information. It is easily affected by dynamic objects, resulting in large errors, and the positioning effect is not stable enough in the long time domain.

[0004] The other type matches the point clouds between two frames, calculates the pose transformation between the point clouds based on the matching results, and then completes the positioning cumulatively. This type of method utilizes the position information of the point cloud target, and the effect is improved. However, since the point cloud of the 4D millimeter wave radar is sampled and generated from the original perception data, its inter-frame stability is poor and it is easily affected by the multipath effect, resulting in poor point cloud quality, high noise and sparseness. Therefore, there will be a large inter-frame matching error, resulting in poor positioning accuracy.

[0005] In summary, in the related technologies, the positioning methods based on 4D millimeter-wave radar are unable to utilize its rich original information, and the positioning effect is relatively unstable and has large errors, which affects the subsequent use of the positioning information and needs to be improved. Summary of the invention

[0006] The present application provides a method, device, vehicle and storage medium for autonomous driving positioning of a vehicle to solve the technical problems in the related art that it is difficult to fully utilize the original information of the 4D millimeter wave radar, the positioning effect is relatively unstable, and there are large errors.

[0007] The first aspect of the present application provides a method for autonomous driving positioning of a vehicle, comprising the following steps: obtaining a 4D millimeter-wave radar tensor of at least one target; calculating the actual speed of the vehicle using the 4D millimeter-wave radar tensor; constructing a posture transformation optimization equation based on the tensor error between adjacent frames, and solving the posture transformation optimization equation to obtain a priori posture transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor; inputting the actual vehicle speed and the priori posture transformation of the current frame into a pre-constructed extended Kalman filter to obtain the observed posture transformation of the vehicle, and obtaining the positioning result of the vehicle based on the observed posture transformation.

[0008] Optionally, in an embodiment of the present application, the obtaining of the 4D millimeter-wave radar tensor of at least one target includes: obtaining an original 4D tensor, where the original 4D tensor is composed of distance, azimuth angle, elevation angle, and Doppler velocity; performing Doppler dimension reduction on the original 4D tensor to obtain the 4D millimeter-wave radar tensor.

[0009] Optionally, in an embodiment of the present application, the calculating of the actual vehicle speed by using the 4D millimeter-wave radar tensor includes: obtaining corresponding target weights based on the 4D millimeter-wave radar tensor and the iteratively reweighted least squares method, and separating dynamic targets and / or static targets from the at least one target according to the target weights; calculating the velocity vector of the vehicle based on the 4D millimeter-wave radar tensor of the static target, and / or calculating the velocity vector based on the 4D millimeter-wave radar tensor of the dynamic target and the radial component of the target velocity of the dynamic target; obtaining the actual vehicle speed based on the velocity vector and the velocity vector of the previous frame.

[0010] Optionally, in an embodiment of the present application, the constructing of a pose transformation optimization equation based on the tensor error between adjacent frames and the solving of the pose transformation optimization equation to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor includes: comparing the 4D millimeter-wave radar tensor with the 4D millimeter-wave radar tensor of the previous frame to obtain the tensor error; constructing a corresponding objective function based on the tensor error and a preset optimization constraint; solving the objective function to obtain the prior pose transformation of the current frame that satisfies the second preset convergence condition.

[0011] Optionally, in an embodiment of the present application, the inputting of the actual vehicle speed and the prior pose transformation of the current frame into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle includes: obtaining the linearized vehicle motion equation and the linearized observation equation corresponding to the posterior pose of the previous frame based on the posterior pose and covariance of the previous frame of the vehicle; estimating the current prior pose and prior covariance of the vehicle according to the actual vehicle speed, the linearized vehicle motion equation, and the linearized observation equation, and calculating the corresponding Kalman gain based on the current prior pose and prior covariance; obtaining the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

[0012] The second aspect of the embodiments of the present application provides an automatic driving positioning device for a vehicle, including: an acquisition module, configured to acquire a 4D millimeter-wave radar tensor of at least one target; a first calculation module, configured to calculate the actual vehicle speed by using the 4D millimeter-wave radar tensor; a second calculation module, configured to construct a pose transformation optimization equation based on the tensor error between adjacent frames, and solve the pose transformation optimization equation to obtain a prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor; a positioning module, configured to input the actual vehicle speed and the prior pose transformation of the current frame into a pre-constructed extended Kalman filter to obtain an observed pose transformation of the vehicle, and obtain a positioning result of the vehicle based on the observed pose transformation.

[0013] Optionally, in an embodiment of the present application, the acquisition module includes: an acquisition unit, configured to acquire an original 4D tensor, where the original 4D tensor is composed of distance, azimuth angle, pitch angle, and Doppler velocity; a dimensionality reduction unit, configured to perform Doppler dimensionality reduction on the original 4D tensor to obtain the 4D millimeter-wave radar tensor.

[0014] Optionally, in an embodiment of the present application, the first calculation module includes: a separation unit, configured to obtain corresponding target weights based on the 4D millimeter-wave radar tensor and the iteratively reweighted least squares method, and separate dynamic targets and / or static targets from the at least one target according to the target weights; a first calculation unit, configured to calculate a velocity vector of the vehicle based on the 4D millimeter-wave radar tensor of the static target, and / or calculate the velocity vector based on the 4D millimeter-wave radar tensor of the dynamic target and the target velocity radial component of the dynamic target; a second calculation unit, configured to obtain the actual vehicle speed based on the velocity vector and the velocity vector of the previous frame.

[0015] Optionally, in an embodiment of the present application, the second calculation unit includes: a third calculation unit, configured to compare the 4D millimeter-wave radar tensor with the 4D millimeter-wave radar tensor of the previous frame to obtain the tensor error; a construction unit, configured to construct a corresponding objective function based on the tensor error and a preset optimization constraint; a fourth calculation unit, configured to solve the objective function to obtain a prior pose transformation of the current frame that satisfies a second preset convergence condition.

[0016] Optionally, in an embodiment of the present application, the positioning module includes: a fifth calculation unit configured to obtain a linearized vehicle motion equation and a linearized observation equation corresponding to the posterior pose of the previous frame of the vehicle based on the posterior pose and covariance of the previous frame of the vehicle; a sixth calculation unit configured to estimate the current prior pose and prior covariance of the vehicle according to the actual vehicle speed, the linearized vehicle motion equation, and the linearized observation equation, and calculate a corresponding Kalman gain based on the current prior pose and prior covariance; a seventh calculation unit configured to obtain the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

[0017] An embodiment of the third aspect of the present application provides a vehicle, including: a memory, a processor, and a computer program stored on the memory and executable on the processor, where the processor executes the program to implement the autonomous driving positioning method of the vehicle as described in the above embodiment.

[0018] An embodiment of the fourth aspect of the present application provides a computer-readable storage medium storing computer instructions for causing a computer to execute the autonomous driving positioning method of the vehicle as described in the above embodiment.

[0019] An embodiment of the fifth aspect of the present application provides a computer program product including a computer program that, when executed, is used to implement the autonomous driving positioning method of the vehicle as described above.

[0020] Embodiments of the present application can calculate the actual vehicle speed of a vehicle according to the 4D millimeter-wave radar tensor of at least one target, and construct a pose transformation optimization equation based on the tensor error between adjacent frames, so as to solve the pose transformation optimization equation to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor. Furthermore, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, and the positioning result of the vehicle is obtained based on the observed pose transformation, making full use of the original information of the 4D millimeter-wave radar to obtain a more accurate positioning result. Thus, the technical problems in the related art that it is difficult to make full use of the original information of the 4D millimeter-wave radar, and the positioning effect is relatively unstable with large errors are solved.

[0021] Additional aspects and advantages of the present application will be given in part in the following description, become apparent in part from the following description, or be learned through the practice of the present application. Description of the Drawings

[0022] The above and / or additional aspects and advantages of the present application will become apparent and be readily understood from the following description of the embodiments in conjunction with the drawings, where:

[0023] Figure 1 Flow chart of an automatic driving positioning method for a vehicle provided according to an embodiment of the present application;

[0024] Figure 2 Format schematic diagram of a 4D millimeter-wave radar tensor according to an embodiment of the present application;

[0025] Figure 3 Principle schematic diagram of an automatic driving positioning method for a vehicle according to an embodiment of the present application;

[0026] Figure 4 Flow chart of an automatic driving positioning method for a vehicle according to an embodiment of the present application;

[0027] Figure 5 Structural schematic diagram of an automatic driving positioning device for a vehicle provided according to an embodiment of the present application;

[0028] Figure 6 Structural schematic diagram of a vehicle provided according to an embodiment of the present application. Detailed implementation manners

[0029] The embodiments of the present application will be described in detail below. Examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to explain the present application and should not be construed as limiting the present application.

[0030] The automatic driving positioning method, device, vehicle, and storage medium of the vehicle according to the embodiments of the present application will be described below with reference to the accompanying drawings. In view of the technical problems in the related art mentioned in the above background art that it is difficult to make full use of the original information of the 4D millimeter-wave radar, and the positioning effect is relatively unstable with large errors, the present application provides an automatic driving positioning method for a vehicle. In this method, the actual vehicle speed can be calculated based on the 4D millimeter-wave radar tensors of at least one target, and a pose transformation optimization equation can be constructed based on the tensor error between adjacent frames, so as to solve the pose transformation optimization equation to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor. Furthermore, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, and the positioning result of the vehicle is obtained based on the observed pose transformation, making full use of the original information of the 4D millimeter-wave radar to obtain a more accurate positioning result. Thus, the technical problems in the related art that it is difficult to make full use of the original information of the 4D millimeter-wave radar, and the positioning effect is relatively unstable with large errors are solved.

[0031] Specifically, Figure 1 Flow schematic diagram of an automatic driving positioning method for a vehicle provided by an embodiment of the present application.

[0032] As Figure 1 shown, the autonomous driving positioning method of the vehicle includes the following steps:

[0033] In step S101, obtain the 4D millimeter-wave radar tensor of at least one target.

[0034] It can be understood that CBDES (Computing Brain Development System, computing platform and development system) consists of two parts: CBB (Computing Brain, computing basic platform) and GAASD (Graphical ADAS - AD Software Developer, graphical software developer). Among them, CBB consists of computing platform hardware, real-time kernel, middleware and functional software. The functional software is the core of this product, aiming to provide basic algorithm components and frameworks for various intelligent driving systems. Based on the innovative solution of "layered decoupling and cross-domain sharing", with the functional software function library as the basis, CBDES cedes the application algorithm development ability to the host manufacturers, supports the host factory engineers to quickly build their own defined intelligent driving systems, and performs function adaptation and parameter tuning. Compared with the existing products and development models, it has three major advantages of "high efficiency / high quality / generative".

[0035] Based on the above system, the embodiments of the present application can build a vehicle intelligent driving system that meets the design requirements. In the embodiments of the present application, it can be used to build an autonomous driving positioning system based on the 4D millimeter-wave radar tensor.

[0036] In the actual execution process, the vehicle can obtain the radar data of multiple targets in the surrounding environment through the 4D millimeter-wave radar during driving, that is, obtain the 4D millimeter-wave radar tensors of multiple environmental targets, so as to realize vehicle positioning by using the 4D millimeter-wave radar tensors.

[0037] Optionally, in an embodiment of the present application, obtaining the 4D millimeter-wave radar tensor of at least one target includes: obtaining an original 4D tensor, where the original 4D tensor is composed of distance, azimuth angle, elevation angle and Doppler velocity; performing Doppler dimension reduction on the original 4D tensor to obtain the 4D millimeter-wave radar tensor.

[0038] Among them, as Figure 2 shown, the four-dimensional tensor of the 4D millimeter-wave radar may include distance r, azimuth angle a, elevation angle e and Doppler velocity v. The value at each position in the tensor represents the object reflection intensity detected by the millimeter-wave radar under the corresponding r, a, e, v values, denoted as φ(r, a, e, v).

[0039] In order to reduce the computational amount, the embodiments of the present application can first perform dimension reduction processing on the input four-dimensional tensor.

[0040] Considering that when r, a, and e are determined, there will be only one definite v in the real environment. Therefore, the embodiments of the present application can reduce the dimension of the Doppler dimension, and take the maximum reflection intensity and its corresponding Doppler velocity at each spatial position as the preprocessed three-dimensional spatial data value, that is:

[0041] ψ(r0,a0,e0) = max v∈V φ(r0,a0,e0,v) (1)

[0042] v(r0,a0,e0) = argmax v∈V φ(r0,a0,e0,v) (2)

[0043] Among them, ψ(r0,a0,e0) is the reflection intensity of each point in the preprocessed three-dimensional space, and v(r0,a0,e0) is the Doppler velocity corresponding to this reflection intensity. They jointly constitute the three-dimensional data after dimension reduction; V is the range of the Doppler velocity v.

[0044] In step S102, the actual vehicle speed is calculated by using the 4D millimeter-wave radar tensor.

[0045] As a possible implementation manner, the embodiments of the present application can calculate the current vehicle speed by using the iterative reweighted least squares method based on the Doppler velocity of the 4D millimeter-wave radar tensor, and optimize the current vehicle speed based on the mean filtering in combination with historical information to obtain the actual vehicle speed.

[0046] Optionally, in an embodiment of the present application, calculating the actual vehicle speed by using the 4D millimeter-wave radar tensor includes: obtaining the corresponding target weights based on the 4D millimeter-wave radar tensor and the iterative reweighted least squares method, and separating dynamic targets and / or static targets from at least one target according to the target weights; calculating the velocity vector of the vehicle based on the 4D millimeter-wave radar tensor of the static target, and / or calculating the velocity vector based on the 4D millimeter-wave radar tensor of the dynamic target and the radial component of the target velocity of the dynamic target; obtaining the actual vehicle speed based on the velocity vector and the velocity vector of the previous frame.

[0047] Among them, the Doppler velocity detected by the 4D millimeter-wave radar is the radial component of the relative velocity of the target with respect to the vehicle. Considering that there may be dynamic objects, the current vehicle speed cannot be directly calculated using it.

[0048] The embodiments of the present application can use the iterative reweighted least squares method, which can filter dynamic objects while calculating the vehicle speed. First, for static targets, there is:

[0049] v(r0,a0,e0) = v s·(sin a0cos e0, cos a0cos e0, sin e0) (3)

[0050] where v s is the vehicle velocity vector. v(r0, a0, e0) is positive towards the vehicle, and a and e are both zero when facing directly forward.

[0051] For a dynamic target, there is a certain deviation between its v(r0, a0, e0) and the ideal radial component of the vehicle velocity v s ·(sin a0cos e0, cos a0cos e0, sin e0). Therefore, weighted by this deviation, an iteratively reweighted least squares problem is constructed as follows:

[0052]

[0053] where is the weight for each position, representing the dynamic degree of the target at that position, and is defined as:

[0054]

[0055] where ∈ is a small quantity to prevent from being too large.

[0056] By iterating formula (4) for a certain number of rounds, the weight of the dynamic target gradually decreases, and a more accurate vehicle velocity v s can be obtained, and can continue to play a weighting role in subsequent matching to improve the matching accuracy.

[0057] After obtaining the current vehicle speed, considering that there may be errors in each frame of detection, therefore, in the actual execution process, error elimination is required to obtain a more accurate actual vehicle speed. Among them, there are various error elimination methods. To improve efficiency, the embodiment of the present application can use mean filtering to average the vehicle speed v s of this frame and the vehicle speed v' s of the previous frame to obtain the actual vehicle speed That is:

[0058]

[0059] In step S103, based on the tensor error between adjacent frames, a pose transformation optimization equation is constructed, and the pose transformation optimization equation is solved to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor.

[0060] Further, embodiments of the present application can compare the current 4D millimeter-wave radar tensor with the previous frame, use the point-by-point reflection intensity in the tensor as a feature to calculate the error between the tensors of the previous frame, construct a pose transformation optimization equation, and use a non-linear optimization method to solve the pose transformation of tensor matching.

[0061] Optionally, in an embodiment of the present application, based on the tensor error between adjacent frames, a pose transformation optimization equation is constructed and solved to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor, including: comparing the 4D millimeter-wave radar tensor with the 4D millimeter-wave radar tensor of the previous frame to obtain a tensor error; constructing a corresponding objective function based on the tensor error and a preset optimization constraint; and solving the objective function to obtain the prior pose transformation of the current frame that satisfies the second preset convergence condition.

[0062] To calculate the vehicle pose transformation, embodiments of the present application can assume that the reflection intensity of the same point in three-dimensional space is the same in different frames. Let the true pose transformation of the vehicle between the front and rear frames be T r , then the coordinates of the same spatial position between two adjacent frames in the millimeter-wave radar coordinate system are p = [r0 a0 e0] T , q = T r p = [r1 a1 e1] T (the fourth dimension of the homogeneous expression is omitted here), then the reflection intensities of the two points should satisfy:

[0063] ψ t (p) = ψ t+1 (q) (7)

[0064] In fact, due to the error in the pose transformation T r existing, equation (7) does not hold. Therefore, the weighted intensity error can be defined as follows:

[0065] I(T) = (ψ t+1 (q) - ψ t (p)) · (λ p + λ q ) (8)

[0066] To obtain the optimal pose estimation, it is necessary to minimize the intensity error (8). Therefore, the objective function is established as:

[0067] min T J(T) = ∑ r∈R ∑ a∈A ∑ e∈E (I(T)) 2 (9)

[0068] Embodiments of the present application can use the Gauss-Newton method for iterative solution. Therefore, it is necessary to solve the following increment equation in each iteration:

[0069] K(T)K T (T)ΔT=-K(T)I(T)(10)

[0070] in,

[0071]

[0072] The derivative of I(T) with respect to T can be obtained by the chain rule:

[0073]

[0074] According to the three-dimensional function derivative, we can solve

[0075]

[0076] According to the Lie algebra (left perturbation), we can solve

[0077]

[0078] Based on the pose transformation of the previous frame, the initial pose transformation T0 of this frame can be set. By combining (10)-(14), ΔT can be obtained. Then, in each iteration, we have:

[0079] T i+1 =ΔT·T i (15)

[0080] In this embodiment of the present application, an iteration threshold T may be set. m , to ΔT <T m It can be considered to have converged. At this moment, T j That is, the pose transformation T of this frame solved based on tensor matching r , that is, the prior pose transformation of the current frame.

[0081] In step S104, the actual vehicle speed and the priori posture transformation of the current frame are input into a pre-built extended Kalman filter to obtain the observed posture transformation of the vehicle, and the positioning result of the vehicle is obtained based on the observed posture transformation.

[0082] During the actual execution process, the embodiment of the present application can construct an EKF (Extended Kalman Filter), input the actual vehicle speed and the prior posture transformation into the EKF, predict the local posture transformation based on the vehicle speed, and then combine the posture transformation calculated by tensor matching to update the posture transformation after EKF observation, so as to obtain real-time local positioning results.

[0083] Optionally, in an embodiment of the present application, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, including: obtaining the linearized vehicle motion equation and the linearized observation equation corresponding to the posterior pose of the previous frame based on the posterior pose and covariance of the previous frame of the vehicle; estimating the current prior pose and prior covariance of the vehicle according to the actual vehicle speed, the linearized vehicle motion equation, and the linearized observation equation, and calculating the corresponding Kalman gain based on the current prior pose and prior covariance; obtaining the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

[0084] As a possible implementation manner, the embodiment of the present application can estimate the actual vehicle speed using Doppler and estimate the prior pose transformation T based on tensor matching r and then construct an extended Kalman filter to update the pose of this frame. Let the posterior pose of the previous frame be and the covariance be The vehicle motion equation is f(T, v), and the observation equation is h(T). Then, first linearize the motion equation and the observation equation at i.e.:

[0085]

[0086] where ω k ~N(0, R k ) is the motion noise, and n k ~N(0, Q k ) is the observation noise. Denote

[0087]

[0088] Then, the prior of the current pose and covariance can be estimated first based on i.e.,

[0089]

[0090] Accordingly, the Kalman gain K k can be calculated as follows:

[0091]

[0092] Then, based on the observed pose transformation T r , the posterior pose estimate and covariance can be obtained as follows:

[0093]

[0094] Combined with Figures 2 to 4As shown, the working principle of the automatic driving positioning method for the vehicle according to the embodiment of the present application is elaborated in detail with an example.

[0095] As Figure 3 shown, in the actual execution process of the embodiment of the present application, first, the original 4D tensor information of the 4D millimeter-wave radar before sampling to generate point cloud is preprocessed, and then based on the Doppler velocity, the iterative reweighted least squares method is used to calculate the current vehicle speed, and the current vehicle speed is optimized based on the mean filter in combination with historical information; in addition, the 4D tensors of the current and the previous frame are compared, and the reflection intensity of each point in the tensor is used as a feature to calculate the error between the tensors of the previous frame, and a pose transformation optimization equation is constructed, and a non-linear optimization method is used to solve the pose transformation of tensor matching. Next, an extended Kalman filter is constructed, and both the current vehicle speed and the pose transformation are input into the filter. Based on the vehicle speed, a prediction is made for the local pose transformation, and then combined with the pose transformation calculated by the tensor matching, the pose transformation after EKF observation is updated, and the real-time local positioning result can be obtained.

[0096] As Figure 4 shown, the overall process of the embodiment of the present application may include the following steps:

[0097] Step S401: Data preprocessing.

[0098] Among them, as Figure 2 shown, the four-dimensional tensor of the 4D millimeter-wave radar may include range r, azimuth angle a, elevation angle e, and Doppler velocity v. The value at each position in the tensor represents the reflection intensity of the object detected by the millimeter-wave radar under the corresponding r, a, e, v values, denoted as φ(r, a, e, v).

[0099] To reduce the computational complexity, the embodiment of the present application may first perform dimensionality reduction processing on the input four-dimensional tensor.

[0100] Considering that in the case where r, a, e are determined, there will be only one determined v in the real environment. Therefore, the embodiment of the present application may perform dimensionality reduction on the Doppler dimension, and take the maximum reflection intensity and its corresponding Doppler velocity at each spatial position as the preprocessed three-dimensional spatial data value, that is:

[0101] ψ(r0, a0, e0) = max v∈V φ(r0, a0, e0, v) (1)

[0102] v(r0, a0, e0) = argmax v∈V φ(r0, a0, e0, v) (2)

[0103] Among them, ψ(r0, a0, e0) is the reflection intensity of each point in the preprocessed three-dimensional space, and v(r0, a0, e0) is the Doppler velocity corresponding to this reflection intensity. Together, they form the three-dimensional data after dimensionality reduction; V is the range of the Doppler velocity v.

[0104] Step S402: Calculate the actual vehicle speed.

[0105] Among them, the Doppler velocity detected by the 4D millimeter-wave radar is the radial component of the relative velocity of the target with respect to the vehicle. Considering that there may be dynamic objects, the current vehicle speed cannot be directly calculated using it.

[0106] The embodiment of the present application can adopt the iteratively reweighted least squares method, which can filter dynamic objects while calculating the vehicle speed. First, for static targets, there is:

[0107] v(r0, a0, e0) = v s ·(sin a0 cos e0, cos a0 cos e0, sin e0) (3)

[0108] Among them, v s is the vehicle velocity vector. v(r0, a0, e0) is positive towards the vehicle, and both a and e are zero in the forward direction.

[0109] For dynamic targets, there is a certain deviation between its v(r0, a0, e0) and the radial component v s ·(sin a0 cos e0, cos a0 cos e0, sin e0) of the ideal vehicle speed. Therefore, weighted by this deviation, an iteratively reweighted least squares problem is constructed as follows:

[0110]

[0111] Among them, is the weight at each position, which characterizes the dynamic degree of the target at this position and is defined as:

[0112]

[0113] Among them, ∈ is a small quantity to prevent from being too large.

[0114] By iterating formula (4) for a certain number of rounds, the weights of dynamic targets gradually decrease, and a more accurate vehicle speed v s , and can continue to play a weighting role in subsequent matching to improve the matching accuracy.

[0115] After obtaining the current vehicle speed, considering that there may be errors in each frame of detection, mean filtering is adopted, and the vehicle speed v of this frame s is averaged with the vehicle speed v' of the previous frame s to obtain the actual vehicle speed That is:

[0116]

[0117] Step S403: Tensor matching pose transformation calculation.

[0118] To calculate the vehicle pose transformation, the embodiments of the present application may assume that the reflection intensity of the same point in three-dimensional space is the same under different frames. Let the true pose transformation of the vehicle between the front and rear frames be T r , then the coordinates of the same spatial position in the millimeter-wave radar coordinate system between two adjacent frames are p = [r0 a0 e0] T , q = T r p = [r1 a1 e1] T (The fourth dimension of the homogeneous expression is omitted here), then the reflection intensities of the two points should satisfy:

[0119] ψ t (p) = ψ t+1 (q)(7)

[0120] Actually, due to the error in the pose transformation T r , the equation (7) does not hold. Therefore, the weighted intensity error can be defined as follows:

[0121] I(T) = (ψ t+1 (q) - ψ t (p)) · (λ p + λ q )(8)

[0122] To obtain the optimal pose estimation, it is necessary to minimize the intensity error (8). Therefore, the objective function is established as:

[0123] min T J(T) = ∑ r∈R ∑ a∈A ∑ e∈E (I(T)) 2 (9)

[0124] The embodiments of the present application can use the Gauss-Newton method for iterative solution. Therefore, it is necessary to solve the following increment equation in each round of iteration:

[0125] K(T)K T (T)ΔT = -K(T)I(T)(10)

[0126] Among them,

[0127]

[0128] The derivative of I(T) with respect to T can be obtained by the chain rule:

[0129]

[0130] According to the three-dimensional function derivative, we can solve

[0131]

[0132] According to the Lie algebra (left perturbation), we can solve

[0133]

[0134] Based on the pose transformation of the previous frame, the initial pose transformation T0 of this frame can be set. By combining (10)-(14), ΔT can be obtained. Then, in each iteration, we have:

[0135] T i+1 =ΔT·T i (15)

[0136] In this embodiment of the present application, an iteration threshold T may be set. m , to ΔT <T m It can be considered to have converged. At this moment, T j That is, the pose transformation T of this frame solved based on tensor matching r , that is, the prior pose transformation of the current frame.

[0137] Step S404: Extended Kalman filter update.

[0138] The embodiment of the present application can use Doppler velocity to estimate the actual vehicle speed. And estimate the prior pose transformation T based on tensor matching r After that, the extended Kalman filter is constructed to update the pose of the frame. Assume that the posterior pose of the previous frame is The covariance is The vehicle motion equation is f(T,v), and the observation equation is h(T). First, The motion equation and observation equation are linearized, namely:

[0139]

[0140] Among them, ω k ~N(0,R k ) is the motion noise, n k ~N(0,Q k ) is the observation noise.

[0141]

[0142] Then, it is possible to first estimate the prior of the current pose and covariance based on That is,

[0143]

[0144] Based on this, the Kalman gain K can be calculated k as follows:

[0145]

[0146] Then, based on the observed pose transformation T r , the posterior pose estimate and covariance can be obtained as follows:

[0147]

[0148] According to the vehicle's autonomous driving positioning method proposed in the embodiments of the present application, the actual vehicle speed can be calculated based on the 4D millimeter-wave radar tensors of at least one target, and a pose transformation optimization equation can be constructed based on the tensor error between adjacent frames, so as to solve the pose transformation optimization equation and obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensors. Furthermore, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, and the positioning result of the vehicle is obtained based on the observed pose transformation, making full use of the original information of the 4D millimeter-wave radar to obtain a more accurate positioning result. Thus, the technical problems in the related art that it is difficult to make full use of the original information of the 4D millimeter-wave radar, and the positioning effect is relatively unstable with large errors are solved.

[0149] Secondly, the vehicle's autonomous driving positioning device proposed in the embodiments of the present application will be described with reference to the accompanying drawings.

[0150] Figure 5 It is a block diagram of the vehicle's autonomous driving positioning device according to the embodiments of the present application.

[0151] As Figure 5 shown, the vehicle's autonomous driving positioning device 10 includes: an acquisition module 100, a first calculation module 200, a second calculation module 300, and a positioning module 400.

[0152] Specifically, the acquisition module 100 is configured to acquire 4D millimeter-wave radar tensors of at least one target.

[0153] The first calculation module 200 is configured to calculate the actual vehicle speed using the 4D millimeter-wave radar tensors.

[0154] The second calculation module 300 is configured to construct a pose transformation optimization equation based on the tensor error between adjacent frames, and solve the pose transformation optimization equation to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor.

[0155] The positioning module 400 is configured to input the actual vehicle speed and the prior pose transformation of the current frame into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, and obtain the positioning result of the vehicle based on the observed pose transformation.

[0156] Optionally, in an embodiment of the present application, the acquisition module 100 includes: an acquisition unit and a dimensionality reduction unit.

[0157] Wherein, the acquisition unit is configured to acquire an original 4D tensor, where the original 4D tensor is composed of distance, azimuth angle, pitch angle, and Doppler velocity.

[0158] The dimensionality reduction unit is configured to perform Doppler dimensionality reduction on the original 4D tensor to obtain a 4D millimeter-wave radar tensor.

[0159] Optionally, in an embodiment of the present application, the first calculation module 200 includes: a separation unit, a first calculation unit, and a second calculation unit.

[0160] Wherein, the separation unit is configured to obtain corresponding target weights based on the 4D millimeter-wave radar tensor and the iteratively reweighted least squares method, and separate dynamic targets and / or static targets from at least one target according to the target weights.

[0161] The first calculation unit is configured to calculate the velocity vector of the vehicle based on the 4D millimeter-wave radar tensor of the static target, and / or calculate the velocity vector based on the 4D millimeter-wave radar tensor of the dynamic target and the radial component of the target velocity of the dynamic target.

[0162] The second calculation unit is configured to obtain the actual vehicle speed based on the velocity vector and the velocity vector of the previous frame.

[0163] Optionally, in an embodiment of the present application, the second calculation unit 300 includes: a third calculation unit, a construction unit, and a fourth calculation unit.

[0164] Wherein, the third calculation unit is configured to compare the 4D millimeter-wave radar tensor with the 4D millimeter-wave radar tensor of the previous frame to obtain a tensor error.

[0165] The construction unit is configured to construct a corresponding objective function based on the tensor error and a preset optimization constraint.

[0166] The fourth calculation unit is configured to solve the objective function to obtain the prior pose transformation of the current frame that satisfies the second preset convergence condition.

[0167] Optionally, in an embodiment of the present application, the positioning module 400 includes: a fifth calculation unit, a sixth calculation unit, and a seventh calculation unit.

[0168] Among them, the fifth calculation unit is configured to obtain a linearized vehicle motion equation and a linearized observation equation corresponding to the posterior pose of the previous frame based on the posterior pose and covariance of the previous frame of the vehicle.

[0169] The sixth calculation unit is configured to estimate the current prior pose and prior covariance of the vehicle according to the actual vehicle speed, the linearized vehicle motion equation, and the linearized observation equation, and calculate the corresponding Kalman gain based on the current prior pose and prior covariance.

[0170] The seventh calculation unit is configured to obtain the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

[0171] It should be noted that the foregoing explanation of the embodiment of the vehicle's autonomous driving positioning method also applies to the vehicle's autonomous driving positioning device in this embodiment, and will not be elaborated here.

[0172] According to the vehicle's autonomous driving positioning device provided by the embodiment of the present application, the actual vehicle speed of the vehicle can be calculated according to the 4D millimeter-wave radar tensor of at least one target, and a pose transformation optimization equation can be constructed based on the tensor error between adjacent frames, so as to solve the pose transformation optimization equation to obtain the prior pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor. Furthermore, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, and the positioning result of the vehicle is obtained based on the observed pose transformation, making full use of the original information of the 4D millimeter-wave radar to obtain a more accurate positioning result. Thus, the technical problems in the related art that it is difficult to make full use of the original information of the 4D millimeter-wave radar, and the positioning effect is relatively unstable with large errors are solved.

[0173] Figure 6 The structural schematic diagram of the vehicle provided by the embodiment of the present application. The vehicle may include:

[0174] A memory 601, a processor 602, and a computer program stored on the memory 601 and executable on the processor 602.

[0175] When the processor 602 executes the program, it implements the vehicle's autonomous driving positioning method provided in the above embodiment.

[0176] Furthermore, the vehicle further includes:

[0177] A communication interface 603 for communication between the memory 601 and the processor 602.

[0178] A memory 601 for storing a computer program that can run on a processor 602.

[0179] The memory 601 may include a high-speed RAM memory and may also include non-volatile memory, such as at least one disk memory.

[0180] If the memory 601, the processor 602, and the communication interface 603 are implemented independently, the communication interface 603, the memory 601, and the processor 602 can be interconnected via a bus and communicate with each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 6 only a thick line is shown in the figure, but it does not mean that there is only one bus or one type of bus.

[0181] Optionally, in a specific implementation, if the memory 601, the processor 602, and the communication interface 603 are integrated on a single chip, the memory 601, the processor 602, and the communication interface 603 can communicate with each other via an internal interface.

[0182] The processor 602 may be a Central Processing Unit (CPU), or an Application Specific Integrated Circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of the present application.

[0183] This embodiment also provides a computer-readable storage medium having a computer program stored thereon, and when the program is executed by a processor, the above-mentioned vehicle's autonomous driving positioning method is implemented.

[0184] The embodiments of the present application also provide a computer program product, including a computer program, and when the computer program is executed by a processor, the vehicle's autonomous driving positioning method provided by the embodiments of the present invention is implemented.

[0185] In the description of this specification, the descriptions referring to terms such as "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of this application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any one or N embodiments or examples in a suitable manner. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0186] In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features. In the description of this application, the meaning of "N" is at least two, such as two, three, etc., unless otherwise specifically defined.

[0187] Any process or method description shown in the flowchart or described in other ways herein can be understood as representing a module, segment, or part of code including one or N executable instructions for realizing a customized logic function or process, and the scope of the preferred embodiments of this application includes additional implementations, where the functions may be executed in a substantially simultaneous manner or in a reverse order according to the involved functions, rather than in the order shown or discussed, which should be understood by those skilled in the art to which the embodiments of this application belong.

[0188] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a definite sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch and execute instructions from the instruction execution system, apparatus, or device), or used in conjunction with these instruction execution systems, apparatuses, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device. More specific examples (non-exhaustive list) of computer-readable media include the following: an electrical connection part (electronic device) having one or N wirings, a portable computer diskette (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically by optically scanning the paper or other media, followed by editing, interpretation, or other appropriate processing as necessary, and then stored in a computer memory.

[0189] It should be understood that various parts of the present application can be implemented by hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or a combination of the following techniques well known in the art can be used: discrete logic circuits having logic gate circuits for implementing logical functions on data signals, application specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field programmable gate arrays (FPGAs), etc.

[0190] Those of ordinary skill in the art of this technology can understand that all or part of the steps carried by the methods of the above embodiments can be completed by instructing relevant hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiments.

[0191] In addition, each functional unit in various embodiments of the present application may be integrated into a processing module, or each unit may exist physically alone, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. When the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0192] The above-mentioned storage medium may be a read-only memory, a magnetic disk or an optical disc, etc. Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present application. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present application.

Claims

1. A method for positioning an automatic driving vehicle, characterized in that: The following steps are involved: Obtain a 4D millimeter-wave radar tensor of at least one target; Calculating the actual speed of the vehicle using the 4D millimeter wave radar tensor; Based on the tensor error between adjacent frames, construct a pose transformation optimization equation, and solve the pose transformation optimization equation to obtain a priori pose transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor; Inputting the actual vehicle speed and the a priori posture transformation of the current frame into a pre-constructed extended Kalman filter to obtain an observed posture transformation of the vehicle, and obtaining a positioning result of the vehicle based on the observed posture transformation; Among them, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-constructed extended Kalman filter to obtain the observed pose transformation of the vehicle, including: based on the posterior pose and covariance of the vehicle in the previous frame, the linearized vehicle motion equation and the linearized observation equation corresponding to the posterior pose of the previous frame are obtained; according to the actual vehicle speed, the linearized vehicle motion equation and the linearized observation equation, the current prior pose and prior covariance of the vehicle are estimated, and the corresponding Kalman gain is calculated based on the current prior pose and prior covariance; based on the Kalman gain and the observed pose transformation, the posterior pose and covariance of the current frame are obtained.

2. The method according to claim 1, characterized in that The obtaining of a 4D millimeter wave radar tensor of at least one target includes: Acquire an original 4D tensor, wherein the original 4D tensor consists of a distance, a direction angle, a pitch angle, and a Doppler velocity; The Doppler dimension reduction is performed on the original 4D tensor to obtain the 4D millimeter-wave radar tensor.

3. The method according to claim 1, characterized in that The calculating the actual speed of the vehicle by using the 4D millimeter wave radar tensor includes: Obtaining corresponding target weights based on the 4D millimeter-wave radar tensor and iteratively reweighted least squares method, and separating dynamic targets and / or static targets from the at least one target according to the target weights; Calculating a velocity vector of the vehicle based on the 4D millimeter-wave radar tensor of the static target, and / or calculating the velocity vector based on the 4D millimeter-wave radar tensor of the dynamic target and a radial component of a target velocity of the dynamic target; The actual vehicle speed is obtained based on the speed vector and the speed vector of the previous frame.

4. The method according to claim 1, characterized in that The method of constructing a posture transformation optimization equation based on the tensor error between adjacent frames, and solving the posture transformation optimization equation to obtain a priori posture transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor, includes: Comparing the 4D millimeter-wave radar tensor with the 4D millimeter-wave radar tensor of the previous frame to obtain the tensor error; Based on the tensor error and the preset optimization constraints, construct a corresponding objective function; Solve the objective function to obtain a priori pose transformation of the current frame that satisfies a second preset convergence condition.

5. A vehicle automatic driving positioning device, characterized in that: include: An acquisition module, used to acquire a 4D millimeter-wave radar tensor of at least one target; A first calculation module, used to calculate the actual speed of the vehicle using the 4D millimeter wave radar tensor; A second calculation module is used to construct a posture transformation optimization equation based on the tensor error between adjacent frames, and solve the posture transformation optimization equation to obtain a priori posture transformation of the current frame of the vehicle that matches the 4D millimeter-wave radar tensor; A positioning module, used for inputting the actual vehicle speed and the priori posture transformation of the current frame into a pre-built extended Kalman filter to obtain an observed posture transformation of the vehicle, and obtaining a positioning result of the vehicle based on the observed posture transformation; Among them, the positioning module includes: a fifth calculation unit, which is used to obtain the linearized vehicle motion equation and linearized observation equation corresponding to the posterior pose of the previous frame based on the posterior pose and covariance of the vehicle in the previous frame; a sixth calculation unit, which is used to estimate the current prior pose and prior covariance of the vehicle according to the actual vehicle speed, the linearized vehicle motion equation and the linearized observation equation, and calculate the corresponding Kalman gain based on the current prior pose and prior covariance; a seventh calculation unit, which is used to obtain the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

6. The device according to claim 5, characterized in that The acquisition module comprises: An acquisition unit, used for acquiring an original 4D tensor, wherein the original 4D tensor consists of a distance, a direction angle, a pitch angle and a Doppler velocity; A dimensionality reduction unit is used to perform Doppler dimension reduction on the original 4D tensor to obtain the 4D millimeter-wave radar tensor.

7. The device according to claim 5, characterized in that The first calculation module includes: A separation unit, configured to obtain a corresponding target weight based on the 4D millimeter-wave radar tensor and an iteratively reweighted least square method, and separate a dynamic target and / or a static target from the at least one target according to the target weight; a first calculation unit, configured to calculate a velocity vector of the vehicle based on a 4D millimeter-wave radar tensor of the static target, and / or calculate the velocity vector based on the 4D millimeter-wave radar tensor of the dynamic target and a radial component of a target velocity of the dynamic target; The second calculation unit is used to obtain the actual vehicle speed based on the speed vector and the speed vector of the previous frame.

8. A vehicle, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the automatic driving positioning method for a vehicle as described in any one of claims 1 to 4.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: The program is executed by a processor to implement the automatic driving positioning method for a vehicle as described in any one of claims 1 to 4.

Images