Automatic driving positioning method and device of vehicle, vehicle and storage medium

Abstract

The invention relates to an automatic driving positioning method and device of a vehicle, the vehicle and a storage medium, and the method comprises the steps: obtaining a 4D millimeter wave radar tensor of at least one target; the actual vehicle speed of the vehicle is calculated through the 4D millimeter wave radar tensor; constructing a pose transformation optimization equation based on the tensor error between the adjacent frames, and solving the pose transformation optimization equation to obtain the prior pose transformation of the current frame of the vehicle matched with the 4D millimeter wave radar tensor; and inputting the actual vehicle speed and the prior pose transformation of the current frame into a pre-constructed extended Kalman filter to obtain observation pose transformation of the vehicle, and obtaining a positioning result of the vehicle based on the observation pose transformation. Therefore, the technical problems that the original information of the 4D millimeter wave radar is difficult to fully utilize, the positioning effect is relatively unstable and a relatively large error exists in the prior art are 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 one embodiment of the present application, obtaining a 4D millimeter-wave radar tensor of at least one target includes: obtaining an original 4D tensor, wherein the original 4D tensor is composed of a distance, a azimuth angle, a pitch angle, and a Doppler velocity; and performing Doppler dimension reduction on the original 4D tensor to obtain the 4D millimeter-wave radar tensor.

[0009] Optionally, in one embodiment of the present application, the calculating the actual speed of the vehicle using the 4D millimeter-wave radar tensor includes: obtaining a corresponding target weight based on the 4D millimeter-wave radar tensor and iterative reweighted least squares method, and separating dynamic targets and / or static targets from the at least one target according to the target weight; 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 one embodiment of the present application, 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, including: 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 preset optimization constraints, a corresponding objective function is constructed; and the objective function is solved to obtain the prior pose transformation of the current frame that meets the second preset convergence condition.

[0011] Optionally, in one 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: based on the posterior pose and covariance of the vehicle in the previous frame, obtaining the linearized vehicle motion equation and linearized observation equation corresponding to the posterior pose of the previous frame; 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 present application provides an automatic driving positioning device for a vehicle, comprising: an acquisition module for acquiring a 4D millimeter-wave radar tensor of at least one target; a first calculation module for calculating the actual speed of the vehicle using the 4D millimeter-wave radar tensor; a second calculation module for 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; a positioning module for 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.

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

[0014] Optionally, in one embodiment of the present application, the first calculation module includes: a separation unit, used to obtain a corresponding target weight based on the 4D millimeter-wave radar tensor and iterative reweighted least squares method, and separate dynamic targets and / or static targets from the at least one target according to the target weight; a first calculation unit, used 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; a second calculation unit, used to obtain the actual vehicle speed based on the velocity vector and the velocity vector of the previous frame.

[0015] Optionally, in one embodiment of the present application, the second computing unit includes: a third computing unit, used 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, used to construct a corresponding objective function based on the tensor error and preset optimization constraints; a fourth computing unit, used to solve the objective function to obtain the prior pose transformation of the current frame that meets the second preset convergence condition.

[0016] Optionally, in one embodiment of the present application, the positioning module includes: a fifth calculation unit, 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, 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, used to obtain the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

[0017] The third aspect of the present application provides a vehicle, comprising: 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 the vehicle as described in the above embodiment.

[0018] A fourth aspect of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the computer to execute the automatic driving positioning method for a vehicle as described in the above embodiments.

[0019] The fifth aspect of the present application provides a computer program product, including a computer program, which, when executed, is used to implement the above-mentioned method for autonomous driving positioning of a vehicle.

[0020] The embodiment of the present application can calculate the actual speed of the vehicle based on the 4D millimeter wave radar tensor of at least one target, and construct the posture transformation optimization equation based on the tensor error between adjacent frames, so as to solve the posture transformation optimization equation, obtain the prior posture transformation of the current frame of the vehicle that matches the 4D millimeter wave radar tensor, and then input the actual speed and the prior posture transformation of the current frame into the pre-constructed extended Kalman filter to obtain the observed posture transformation of the vehicle, and obtain the positioning result of the vehicle based on the observed posture transformation, making full use of the original information of the 4D millimeter wave radar, and obtaining a more accurate positioning result. 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 and has large errors is solved.

[0021] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through the practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

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

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

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

[0028] Figure 6 It is a schematic diagram of the structure of a vehicle provided according to an embodiment of the present application. DETAILED DESCRIPTION

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

[0030] The following describes the automatic driving positioning method, device, vehicle and storage medium of the vehicle of the embodiment of the present application with reference to the accompanying drawings. In view of the technical problem that it is difficult to make full use of the original information of the 4D millimeter wave radar in the related technology mentioned in the above background technology, the positioning effect is relatively unstable, and there are large errors, the present application provides a method for automatic driving positioning of a vehicle, in which the actual speed of the vehicle can be calculated according to the 4D millimeter wave radar tensor of at least one target, and the posture transformation optimization equation is constructed based on the tensor error between adjacent frames, so as to solve the posture transformation optimization equation, and obtain the prior posture transformation of the current frame of the vehicle matching the 4D millimeter wave radar tensor, and then the actual speed and the prior posture transformation of the current frame are input into the pre-constructed 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, so as to make full use of the original information of the 4D millimeter wave radar and obtain a more accurate positioning result. Thus, the technical problem that it is difficult to make full use of the original information of the 4D millimeter wave radar, the positioning effect is relatively unstable, and there are large errors in the related technology is solved.

[0031] Specifically, Figure 1 A flowchart of an automatic driving positioning method for a vehicle provided in an embodiment of the present application.

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

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

[0034] It is understandable that CBDES (Computing Brain Development System) consists of two parts: CBB (Computing Brain) and GAASD (Graphical ADAS-AD Software Developer). CBB consists of computing platform hardware, real-time kernel, middleware and functional software. Functional software is the core of this product, which aims to provide basic algorithm components and frameworks for various intelligent driving systems. Based on the innovative solution of "layered decoupling and cross-domain sharing" and based on the functional software function library, CBDES transfers the application algorithm development capability to the host manufacturers, supports the engineers of the host manufacturers to quickly build their own defined intelligent driving systems, and perform function adaptation and parameter adjustment. Compared with existing products and development models, it has the three advantages of "high efficiency / high quality / generative".

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

[0036] In the actual implementation process, the vehicle can obtain radar data of multiple targets in the surrounding environment through the 4D millimeter-wave radar while driving, that is, obtain the 4D millimeter-wave radar tensor of multiple environmental targets, and then use the 4D millimeter-wave radar tensor to realize vehicle positioning.

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

[0038] Among them, Figure 2 As shown, the thought tensor of the 4D millimeter-wave radar can include distance r, direction angle a, pitch angle e and Doppler velocity v. The value of each position in the tensor represents the reflection intensity of the object detected by the millimeter-wave radar at the corresponding r, a, e, v values, which is recorded as φ(r, a, e, v).

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

[0040] Considering that when r, a, and e are determined, there will only be one certain v in the real environment. Therefore, the embodiment of the present application can reduce the Doppler dimension and take the maximum reflection intensity and its corresponding Doppler velocity in 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 three-dimensional space after preprocessing, v(r0,a0,e0) is the Doppler velocity corresponding to the reflection intensity, and they together constitute the three-dimensional data after dimensionality reduction; V is the range of Doppler velocity v.

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

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

[0046] Optionally, in one embodiment of the present application, the actual speed of the vehicle is calculated using a 4D millimeter-wave radar tensor, including: obtaining a corresponding target weight based on the 4D millimeter-wave radar tensor and iterative reweighted least squares method, and separating a dynamic target and / or a static target from at least one target according to the target weight; calculating the vehicle's velocity vector 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 to the vehicle. Considering the possible existence of dynamic objects, it cannot be used directly to calculate the current vehicle speed.

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

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

[0050] Among them, v s is the vehicle velocity vector. v(r0,a0,e0) is positive when it is facing the vehicle, and a and e are zero when they are facing straight ahead.

[0051] For dynamic targets, v(r0,a0,e0) and the ideal vehicle velocity radial component v s There is a certain deviation between (sin a0cos e0, cos a0cos e0, sin e0), so the deviation is weighted to construct the iterative reweighted least squares problem as follows:

[0052]

[0053] in, The weight of each position characterizes the dynamic degree of the target at that position and is defined as:

[0054]

[0055] Among them, ∈ is a prevention Too small a quantity.

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

[0057] After obtaining the current speed of the vehicle, considering that there may be errors in each frame detection, it is necessary to eliminate the errors in the actual execution process to obtain a more accurate actual speed. There are many error elimination methods. In order to improve efficiency, the embodiment of the present application can use mean filtering to convert the frame speed v s and the vehicle speed v′ of the previous frame s Find the average and get the actual speed Right now:

[0058]

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

[0060] Furthermore, the embodiment of the present application can compare the current and previous frames of 4D millimeter-wave radar tensors, use the point-by-point reflection intensity in the tensor as a feature to calculate the error between the tensor and the previous frame, construct a posture transformation optimization equation, and use a nonlinear optimization method to solve the posture transformation of the tensor matching.

[0061] Optionally, in one embodiment of the present application, 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 a 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; based on the tensor error and preset optimization constraints, constructing a corresponding objective function; solving the objective function to obtain a prior pose transformation of the current frame that meets a second preset convergence condition.

[0062] In order to calculate the vehicle posture transformation, the embodiment of the present application can assume that the reflection intensity of the same point in the three-dimensional space in different frames is the same, and the actual posture transformation of the vehicle in the previous and next frames is set to 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), the reflection intensity of the two points should satisfy:

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

[0064] In fact, due to the posture transformation T r There is an error, equation (7) does not hold, so the weighted intensity error can be defined as follows:

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

[0066] In order to obtain the optimal pose estimation, the intensity error (8) needs to be minimized, so the objective function is established as:

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

[0068] The embodiment of the present application can use the Gauss-Newton method to iteratively solve, so the following incremental equation needs to be solved in each round of 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 one embodiment of the present application, the actual vehicle speed and the prior pose transformation of the current frame are input into a pre-built 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, obtaining the linearized vehicle motion equation and linearized observation equation corresponding to the posterior pose of the previous frame; 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 method, 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:

[0085]

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

[0087]

[0088] First, we can base Estimate the prior of the current pose and covariance, i.e.

[0089]

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

[0091]

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

[0093]

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

[0095] like Figure 3 As shown, in the actual implementation process of the embodiment of the present application, first, the original 4D tensor information of the 4D millimeter-wave radar before sampling to generate the 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 the historical information; in addition, the 4D tensors of the current and previous frames are compared, and the point-by-point reflection intensity in the tensor is used as a feature to calculate the error between the tensor and the previous frame, to construct the posture transformation optimization equation, and to use the nonlinear optimization method to solve the posture transformation of the tensor matching. Next, an extended Kalman filter is constructed, and the current vehicle speed and posture transformation are input into the filter. The local posture transformation is predicted based on the vehicle speed, and then the posture transformation calculated by the tensor matching is combined to update the posture transformation after the EKF observation, and the real-time local positioning result can be obtained.

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

[0097] Step S401: data preprocessing.

[0098] Among them, Figure 2 As shown, the thought tensor of the 4D millimeter-wave radar can include distance r, direction angle a, pitch angle e and Doppler velocity v. The value of each position in the tensor represents the reflection intensity of the object detected by the millimeter-wave radar at the corresponding r, a, e, v values, which is recorded as φ(r, a, e, v).

[0099] In order to reduce the amount of calculation, the embodiment of the present application can first perform dimensionality reduction processing on the input four-dimensional tensor.

[0100] Considering that when r, a, and e are determined, there will only be one certain v in the real environment. Therefore, the embodiment of the present application can reduce the Doppler dimension and take the maximum reflection intensity and its corresponding Doppler velocity in 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 three-dimensional space after preprocessing, v(r0,a0,e0) is the Doppler velocity corresponding to the reflection intensity, and they together constitute the three-dimensional data after dimensionality reduction; V is the range of Doppler velocity v.

[0104] Step S402: Calculate 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 to the vehicle. Considering the possible existence of dynamic objects, it cannot be used directly to calculate the current vehicle speed.

[0106] The embodiment of the present application can use iterative reweighted least squares method to filter dynamic objects while calculating the vehicle speed. First, for static targets, there are:

[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 when it is facing the vehicle, and a and e are zero when they are facing straight ahead.

[0109] For dynamic targets, v(r0,a0,e0) and the ideal vehicle velocity radial component v s There is a certain deviation between (sin a0 cos e0, cos a0 cos e0, sin e0), so the deviation is weighted to construct the iterative reweighted least squares problem as follows:

[0110]

[0111]

[0112] in, The weight of each position characterizes the dynamic degree of the target at that position and is defined as:

[0113]

[0114] Among them, ∈ is a prevention Too small a quantity.

[0115] By iterating formula (4) for a certain number of rounds, the weight of the dynamic target is gradually reduced, and a more accurate vehicle speed v can be obtained. s ,and It can continue to play a weighted role in subsequent matching to improve matching accuracy.

[0116] After obtaining the current speed of the vehicle, considering that there may be errors in each frame detection, mean filtering is used to convert the frame speed v s and the vehicle speed v′ of the previous frame s Find the average and get the actual speed Right now:

[0117]

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

[0119] In order to calculate the vehicle posture transformation, the embodiment of the present application can assume that the reflection intensity of the same point in the three-dimensional space in different frames is the same, and the actual posture transformation of the vehicle in the previous and next frames is set to 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), the reflection intensity of the two points should satisfy:

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

[0121] In fact, due to the posture transformation T r There is an error, equation (7) does not hold, so the weighted intensity error can be defined as follows:

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

[0123] In order to obtain the optimal pose estimation, the intensity error (8) needs to be minimized, so the objective function is established as:

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

[0125] The embodiment of the present application can use the Gauss-Newton method to iteratively solve, so the following incremental equation needs to be solved in each round of iteration:

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

[0127] in,

[0128]

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

[0130]

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

[0132]

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

[0134]

[0135] 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:

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

[0137] 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.

[0138] Step S404: Extended Kalman filter update.

[0139] 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:

[0140]

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

[0142]

[0143] First, we can base Estimate the prior of the current pose and covariance, i.e.

[0144]

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

[0146]

[0147] Then based on the observed pose transformation T r , we can get the posterior pose estimate and covariance as follows:

[0148]

[0149] According to the automatic driving positioning method of the vehicle proposed in the embodiment of the present application, the actual speed of the vehicle can be calculated based on the 4D millimeter wave radar tensor of at least one target, and based on the tensor error between adjacent frames, the pose transformation optimization equation is constructed, thereby solving 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, and then the actual speed and the prior pose transformation of the current frame are input into the 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 problem 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 is solved.

[0150] Next, the automatic driving positioning device for a vehicle proposed in accordance with an embodiment of the present application will be described with reference to the accompanying drawings.

[0151] Figure 5 It is a block diagram of an automatic driving positioning device for a vehicle according to an embodiment of the present application.

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

[0153] Specifically, the acquisition module 100 is used to acquire a 4D millimeter-wave radar tensor of at least one target.

[0154] The first calculation module 200 is used to calculate the actual speed of the vehicle using the 4D millimeter wave radar tensor.

[0155] The second calculation module 300 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.

[0156] The positioning module 400 is used to input the actual vehicle speed and the priori posture transformation of the current frame into a pre-built extended Kalman filter to obtain the observed posture transformation of the vehicle, and obtain the positioning result of the vehicle based on the observed posture transformation.

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

[0158] The acquisition unit is used to acquire the original 4D tensor, wherein the original 4D tensor consists of distance, direction angle, pitch angle and Doppler velocity.

[0159] The dimension reduction unit is used to reduce the Doppler dimension of the original 4D tensor to obtain a 4D millimeter-wave radar tensor.

[0160] Optionally, in one embodiment of the present application, the first computing module 200 includes: a separation unit, a first computing unit and a second computing unit.

[0161] Among them, the separation unit is used to obtain the corresponding target weight based on the 4D millimeter wave radar tensor and the iterative reweighted least squares method, and separate the dynamic target and / or the static target from at least one target according to the target weight.

[0162] The first calculation unit is used 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.

[0163] 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.

[0164] Optionally, in one embodiment of the present application, the second computing unit 300 includes: a third computing unit, a construction unit and a fourth computing unit.

[0165] The third computing unit is used to compare the 4D millimeter-wave radar tensor with the 4D millimeter-wave radar tensor of the previous frame to obtain a tensor error.

[0166] The construction unit is used to construct the corresponding objective function based on the tensor error and the preset optimization constraints.

[0167] The fourth computing unit is used to solve the objective function and obtain the prior pose transformation of the current frame that meets the second preset convergence condition.

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

[0169] Among them, the fifth calculation unit is used to obtain the linearized vehicle motion equation and linearized observation equation corresponding to the posterior posture of the previous frame based on the posterior posture and covariance of the vehicle in the previous frame.

[0170] The sixth calculation unit is used to estimate the current prior posture 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 posture and prior covariance.

[0171] The seventh computing unit is used to obtain the posterior pose and covariance of the current frame based on the Kalman gain and the observed pose transformation.

[0172] It should be noted that the aforementioned explanation of the embodiment of the vehicle's autonomous driving positioning method is also applicable to the vehicle's autonomous driving positioning device of this embodiment, and will not be repeated here.

[0173] According to the automatic driving positioning device for a vehicle proposed in the embodiment of the present application, the actual speed of the vehicle can be calculated based on the 4D millimeter wave radar tensor of at least one target, and based on the tensor error between adjacent frames, a posture transformation optimization equation is constructed, thereby solving the posture transformation optimization equation to obtain the prior posture transformation of the current frame of the vehicle that matches the 4D millimeter wave radar tensor, and then the actual speed and the prior posture transformation of the current frame are input into the pre-constructed 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, making full use of the original information of the 4D millimeter wave radar to obtain a more accurate positioning result. Thus, the technical problem 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 is solved.

[0174] Figure 6 A schematic diagram of the structure of a vehicle provided in an embodiment of the present application. The vehicle may include:

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

[0176] When the processor 602 executes the program, the automatic driving positioning method for the vehicle provided in the above embodiment is implemented.

[0177] Furthermore, the vehicle also includes:

[0178] The communication interface 603 is used for communication between the memory 601 and the processor 602 .

[0179] The memory 601 is used to store computer programs that can be executed on the processor 602 .

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

[0181] 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 connected to each other through a bus and communicate with each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component (PCI) bus or an Extended Industry Standard Architecture (EISA) bus. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 6 Only one thick line is used in the diagram, but this does not mean that there is only one bus or only one type of bus.

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

[0183] 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.

[0184] This embodiment also provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the above-mentioned method for autonomous driving positioning of a vehicle is implemented.

[0185] An embodiment of the present application also provides a computer program product, including a computer program, which, when executed by a processor, implements the automatic driving positioning method for a vehicle provided in an embodiment of the present invention.

[0186] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present 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, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0187] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of this application, "N" means at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0188] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or N executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present application belong.

[0189] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in combination with these instruction execution systems, devices or apparatuses. For the purpose of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in combination with these instruction execution systems, devices or apparatuses. More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or N wirings (electronic devices), a portable computer disk box (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), a fiber optic device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically by optically scanning the paper or other medium and then editing, interpreting or processing in other suitable ways as necessary and then storing it in a computer memory.

[0190] It should be understood that the various parts of the present application can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiment, 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 by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0191] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0192] In addition, each functional unit in each embodiment of the present application may be integrated into a processing module, or each unit may exist physically separately, 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. If 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.

[0193] The storage medium mentioned above may be a read-only memory, a magnetic disk or an optical disk, 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 cannot be understood as limiting the present application. A person of ordinary skill in the art may change, modify, replace and modify 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; The actual vehicle speed and the priori posture transformation of the current frame are input into a pre-constructed 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.

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. The method according to claim 1, characterized in that: The step of inputting the actual vehicle speed and the priori posture transformation of the current frame into a pre-built extended Kalman filter to obtain the observed posture transformation of the vehicle includes: Based on the posterior pose and covariance of the vehicle in the previous frame, a linearized vehicle motion equation and a linearized observation equation corresponding to the posterior pose of the previous frame are obtained; estimating a current a priori pose and a priori covariance of the vehicle according to the actual vehicle speed, the linearized vehicle motion equation and the linearized observation equation, and calculating a corresponding Kalman gain based on the current a priori pose and the prior covariance; The posterior pose and covariance of the current frame are obtained based on the Kalman gain and the observed pose transformation.

6. 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 is used to input the actual vehicle speed and the prior posture transformation of the current frame into a pre-built extended Kalman filter to obtain the observed posture transformation of the vehicle, and obtain the positioning result of the vehicle based on the observed posture transformation.

7. The device according to claim 6, 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.

8. The device according to claim 6, 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.

9. 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 5.

10. 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-5.

Images