Method, device and equipment for calibrating sensing speed of laser radar by millimeter wave radar

Abstract

The invention relates to the technical field of vehicles, in particular to a method, device and equipment for calibrating the sensing speed of a laser radar through a millimeter wave radar, and the method comprises the steps: building a motion state model of a to-be-observed target; acquiring laser radar observation data and millimeter wave radar observation data of a to-be-observed target, and predicting state estimation of the to-be-observed target at the next moment by using the motion state model; using a first preset Kalman filter to correct the state estimation based on the laser radar observation data to obtain the corrected state estimation, and using a second preset Kalman filter to optimize the corrected state estimation based on the millimeter wave radar observation data to obtain the target state estimation. Therefore, the problem that the speed sensing capability of the laser radar is limited is solved, and the speed tracking capability of the laser radar is improved by fusing the obstacle speed data sensed by the millimeter wave radar into the sensing data of the laser radar.

Description

Technical Field

[0001] The present application relates to the field of vehicle technology, and in particular to a method, device and equipment for calibrating a millimeter-wave radar to sense speed using a laser radar. Background Art

[0002] LiDAR, based on the time-of-flight principle, offers high accuracy in ranging and three-dimensional positioning. However, its direct velocity measurement capability is limited, typically relying on inter-frame differentials to calculate velocity and susceptible to motion distortion and noise. 4D millimeter-wave radar (4DRadar), on the other hand, directly measures the radial velocity of a target using the Doppler effect, offering greater velocity stability but lower angular resolution and insufficient ranging accuracy.

[0003] Related technologies usually use lidar to sense the speed of obstacles, but lidar's speed measurement capabilities are limited and it is difficult to meet the perception requirements of complex environments. Summary of the Invention

[0004] The present application provides a method, device, electronic device and storage medium for calibrating the speed of a laser radar using a millimeter-wave radar to solve the problem of the limited speed perception capability of the laser radar. By integrating the obstacle speed data perceived by the millimeter-wave radar into the perception data of the laser radar, the speed tracking capability of the laser radar is improved.

[0005] A first embodiment of the present application provides a method for calibrating a millimeter-wave radar to sense speed using a laser radar, comprising the following steps:

[0006] Establish a motion state model of the target to be observed;

[0007] Obtaining laser radar observation data and millimeter wave radar observation data of the target to be observed, and using the motion state model to predict a state estimate of the target to be observed at the next moment;

[0008] Using a first preset Kalman filter, the state estimate is corrected based on the lidar observation data to obtain a corrected state estimate, and using a second preset Kalman filter, the corrected state estimate is optimized based on the millimeter-wave radar observation data to obtain a target state estimate.

[0009] Optionally, in some embodiments, the motion state model is:

[0010] x k =F k-1 x k-1 +w k-1 ;

[0011] Among them, x k is the state equation at time k, F k-1 is the state transfer matrix at time k-1, xk-1 is the state equation at time k-1, w k-1 is the process noise at time k-1.

[0012] Optionally, in some embodiments, the laser radar observation data is:

[0013]

[0014] in, is the observation value of the laser radar at time k, H LiDAR is the observation matrix of the lidar, is the observation noise of the lidar at time k.

[0015] Optionally, in some embodiments, the first preset Kalman filter is:

[0016] K LiDAR =P k|k-1 (H LiDAR ) T (H LiDAR P k|k-1 (H LiDAR ) T +R LiDAR ) -1 ;

[0017] Among them, J LiDAR is the first Kalman gain, P k|k-1 is the prediction covariance matrix, R LiDAR is the observation noise of the lidar.

[0018] Optionally, in some embodiments, the second preset Kalman filter is:

[0019]

[0020] Among them, K Radar is the second Kalman gain, is the updated state covariance matrix of the lidar, H Radar is the millimeter wave radar observation matrix, R Radar is the observation noise of millimeter-wave radar.

[0021] A second embodiment of the present application provides a device for calibrating a millimeter-wave radar to sense laser radar speed, comprising:

[0022] Establishing a module for establishing a motion state model of the target to be observed;

[0023] An acquisition module, configured to acquire the laser radar observation data and the millimeter wave radar observation data of the target to be observed, and predict the state estimation of the target to be observed at the next moment using the motion state model;

[0024] An estimation module is used to use a first preset Kalman filter to correct the state estimate based on the lidar observation data to obtain a corrected state estimate, and to use a second preset Kalman filter to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate.

[0025] Optionally, in some embodiments, the motion state model is:

[0026] x k =F k-1 x k-1 +w k-1 ;

[0027] Among them, x k is the state equation at time x, F k-1 is the state transfer matrix at time k-1, k k-1 is the state equation at time k-1, w k-1 is the process noise at time k-1.

[0028] Optionally, in some embodiments, the laser radar observation data is:

[0029]

[0030] in, is the observation value of the laser radar at time k, H LiDAR is the observation matrix of the lidar, is the observation noise of the lidar at time k.

[0031] Optionally, in some embodiments, the first preset Kalman filter is:

[0032] K LiDAR =P k|k-1 (H LiDAR ) T (H LiDAR P k|k-1 (H LiDAR ) T +R LiDAR ) -1 ;

[0033] Among them, K LiDAR is the first Kalman gain, P k|k-1 is the prediction covariance matrix, R LiDAR is the observation noise of the lidar.

[0034] Optionally, in some embodiments, the second preset Kalman filter is:

[0035]

[0036] Among them, K Radar is the second Kalman gain, is the updated state covariance matrix of the lidar, H Radar is the millimeter wave radar observation matrix, R Radar is the observation noise of millimeter-wave radar.

[0037] The third aspect of the present application provides an electronic device, comprising: a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor executes the program to implement the method for calibrating the laser radar perception speed by the millimeter-wave radar as described in the above embodiment.

[0038] The fourth aspect of the present application provides a computer-readable storage medium having a computer program stored thereon, which is executed by a processor to implement the method of calibrating the millimeter-wave radar to sense the speed of the laser radar as described in the above embodiment.

[0039] Thus, by establishing a motion state model of the target to be observed and acquiring both lidar and millimeter-wave radar observation data of the target, the motion state model is used to predict the target's state estimate at the next moment. A first preset Kalman filter is used to correct the state estimate based on the lidar observation data to obtain a corrected state estimate. A second preset Kalman filter is then used to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate. This solves the limited velocity sensing capability of lidar and improves its velocity tracking capability by integrating obstacle velocity data sensed by millimeter-wave radar into the lidar's perception data.

[0040] 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 practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0042] Figure 1 A flowchart of a method for calibrating a millimeter-wave radar to sense laser radar speed according to an embodiment of the present application;

[0043] Figure 2 A block diagram of an apparatus for calibrating a millimeter-wave radar to sense laser radar speed according to an embodiment of the present application;

[0044] Figure 3 A schematic diagram of the structure of an electronic device provided according to an embodiment of the present application. DETAILED DESCRIPTION

[0045] The following describes in detail embodiments of the present application, examples of which 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.

[0046] The following describes the method, device and equipment for calibrating the speed of the laser radar by the millimeter-wave radar of the embodiment of the present application with reference to the accompanying drawings. In response to the problem of limited speed perception capability of the laser radar mentioned in the above background technology, the present application provides a method for calibrating the speed of the laser radar by the millimeter-wave radar. In this method, a motion state model of the target to be observed is established, and the laser radar observation data and millimeter-wave radar observation data of the target to be observed are obtained. The motion state model is used to predict the state estimate of the target to be observed at the next moment, and a first preset Kalman filter is used to correct the state estimate based on the laser radar observation data to obtain a corrected state estimate, and a second preset Kalman filter is used to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate. In this way, the problem of limited speed perception capability of the laser radar is solved, and the speed tracking capability of the laser radar is improved by fusing the obstacle speed data perceived by the millimeter-wave radar into the perception data of the laser radar.

[0047] Specifically, Figure 1 A flowchart of a method for calibrating a laser radar to sense speed using a millimeter-wave radar is provided in an embodiment of the present application.

[0048] like Figure 1 As shown, the method for calibrating the millimeter-wave radar to sense the speed of the laser radar includes the following steps:

[0049] In step S101 , a motion state model of the target to be observed is established.

[0050] Specifically, assume that the state vector of the target is

[0051] Among them, x, y, z represent the position of the target in three-dimensional space, v x 、v y 、v z represents the speed of the target in three-dimensional space, a x 、a y 、a z Indicates the acceleration of the target in three-dimensional space.

[0052] Among them, the state equation of the target is:

[0053] x k=F k-1 x k-1 +w k-1 ;

[0054] F k-1 is the state transfer matrix, the obstacle motion model adopts the uniform acceleration model, and the single-step state step length is Δt. w k-1 is process noise, which obeys Gaussian distribution The acceleration noise in the process is white noise and obeys Gaussian distribution. is the variance of the acceleration noise.

[0055] In step S102, the laser radar observation data and the millimeter wave radar observation data of the target to be observed are obtained, and the state estimation of the target to be observed at the next moment is predicted using the motion state model.

[0056] Specifically, the lidar mainly measures the target position (x, y, z). The observation matrix of the lidar is:

[0057]

[0058] Among them, H is the observation matrix, v is the observation noise, and it obeys the Gaussian distribution v~N(0,R LiDAR ), R LiDAR is the covariance of the observation noise, σ x ,σ y ,σ z is the measurement error of LIDAR in three directions.

[0059] The observation matrix of 4D millimeter wave radar is:

[0060]

[0061] Among them, H Radar is the observation matrix, V is the observation noise, and it obeys the Gaussian distribution v~N(0, R RADAR ), 4D millimeter wave radar measures radial distance r, azimuth angle θ, pitch angle φ, radial velocity v r (non-linear observation):

[0062]

[0063] Among them, the nonlinear function h(x k ) is defined as;

[0064]

[0065]

[0066]

[0067]

[0068] The Extended Kalman Filter (EKF) requires calculation of the Jacobian matrix:

[0069]

[0070] 4DRADAR observation noise covariance matrix

[0071] In step S103, a first preset Kalman filter is used to correct the state estimate based on the lidar observation data to obtain a corrected state estimate, and a second preset Kalman filter is used to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate.

[0072] Specifically, EKF is used to fuse 4D RADAR and LiDAR data.

[0073] (1) Prediction stage:

[0074]

[0075] P k|k-1 =FP k-1|k-1 F T +Q;

[0076] (2) Update phase (LiDAR):

[0077] Kalman gain:

[0078] K LiDAR =P k|k-1 (H LiDAR ) T (H LiDAR P k|k-1 (H LiDAR ) T +R LiDAR ) -1 ;

[0079] Status Update:

[0080] Covariance update:

[0081] When the covariance At the minimum value, the perceived speed error of LIDAR fusion 4DRADAR is the smallest.

[0082] Perform EKF calculation on RADAR to calculate the Jacobian matrix H Radar exist The value at .

[0083] Kalman gain:

[0084] Status Update:

[0085] Covariance update:

[0086] The above steps are repeated P k|k When the Kalman gain K is at its minimum, it is substituted into the LiDAR state equation and the RADAR state equation to obtain the optimal target position and velocity after fusion.

[0087] According to the method for calibrating lidar speed perception using a millimeter-wave radar, as proposed in an embodiment of the present application, a motion state model of the target to be observed is established, and lidar observation data and millimeter-wave radar observation data of the target to be observed are obtained. The motion state model is used to predict the state estimate of the target to be observed at the next moment. A first preset Kalman filter is used to correct the state estimate based on the lidar observation data to obtain a corrected state estimate. A second preset Kalman filter is used to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate. This solves the problem of lidar's limited speed perception capability and improves the lidar's speed tracking capability by integrating obstacle speed data sensed by the millimeter-wave radar into the lidar's perception data.

[0088] Next, a device for calibrating the speed of a laser radar using a millimeter-wave radar according to an embodiment of the present application will be described with reference to the accompanying drawings.

[0089] Figure 2 It is a block diagram of an apparatus for calibrating a millimeter-wave radar to sense laser radar speed according to an embodiment of the present application.

[0090] like Figure 2 As shown, the device 10 for calibrating the millimeter-wave radar to sense the speed of the laser radar includes: an establishment module 100, an acquisition module 200 and an estimation module 300.

[0091] The establishment module 100 is used to establish a motion state model of the target to be observed.

[0092] The acquisition module 200 is used to acquire the laser radar observation data and the millimeter wave radar observation data of the target to be observed, and use the motion state model to predict the state estimation of the target to be observed at the next moment.

[0093] The estimation module 300 is used to use a first preset Kalman filter to correct the state estimate based on the lidar observation data to obtain a corrected state estimate, and to use a second preset Kalman filter to optimize the corrected state estimate based on the millimeter wave radar observation data to obtain a target state estimate.

[0094] Optionally, in some embodiments, the motion state model is:

[0095] x k =F k-1 x k-1 +w k-1 ;

[0096] Among them, x k is the state equation at time k, F k-1 is the state transfer matrix at time k-1, x k-1 is the state equation at time k-1, w k-1 is the process noise at time k-1.

[0097] Optionally, in some embodiments, the laser radar observation data is:

[0098]

[0099] in, is the observation value of the laser radar at time k, H LiDAR is the observation matrix of the lidar, is the observation noise of the lidar at time k.

[0100] Optionally, in some embodiments, the first preset Kalman filter is:

[0101] K LiDAR =P k|k-1 (H LiDAR ) T (H LiDAR P k|k-1 (H LiDAR ) T +R LiDAR ) -1 ;

[0102] Among them, K LiDAR is the first Kalman gain, P k|k-1 is the prediction covariance matrix, R LiDAR is the observation noise of the lidar.

[0103] Optionally, in some embodiments, the second preset Kalman filter is:

[0104]

[0105] Among them, KRadar is the second Kalman gain, is the updated state covariance matrix of the lidar, H Radar is the millimeter wave radar observation matrix, R Radar is the observation noise of millimeter-wave radar.

[0106] It should be noted that the above explanation of the embodiment of the method for calibrating the millimeter-wave radar to sense the speed of the laser radar is also applicable to the device for calibrating the millimeter-wave radar to sense the speed of the laser radar of this embodiment, and will not be repeated here.

[0107] According to the device for calibrating the speed perception of a millimeter-wave radar with a lidar, a motion state model of the target to be observed is established, and the lidar observation data and millimeter-wave radar observation data of the target to be observed are obtained. The motion state model is used to predict the state estimate of the target to be observed at the next moment. A first preset Kalman filter is used to correct the state estimate based on the lidar observation data to obtain a corrected state estimate. A second preset Kalman filter is used to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate. This solves the problem of the limited speed perception capability of the lidar, and improves the speed tracking capability of the lidar by integrating the obstacle speed data perceived by the millimeter-wave radar into the perception data of the lidar.

[0108] Figure 3 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. The electronic device may include:

[0109] Memory 301 , processor 302 , and computer programs stored in the memory 301 and executable on the processor 302 .

[0110] When the processor 302 executes the program, the method for calibrating the millimeter-wave radar with the laser radar to sense speed provided in the above embodiment is implemented.

[0111] Furthermore, the electronic device further includes:

[0112] The communication interface 303 is used for communication between the memory 301 and the processor 302 .

[0113] The memory 301 is used to store computer programs that can be run on the processor 302 .

[0114] The memory 301 may include a high-speed RAM (Random Access Memory) memory, and may also include a non-volatile memory, such as at least one disk memory.

[0115] If the memory 301, processor 302, and communication interface 303 are implemented independently, the communication interface 303, memory 301, and processor 302 can be connected to each other via a bus and communicate with each other. The bus can be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, or an EISA (Extended Industry Standard Architecture) bus. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 3 Only one thick line is used in the diagram, but this does not mean that there is only one bus or one type of bus.

[0116] Optionally, in a specific implementation, if the memory 301, the processor 302 and the communication interface 303 are integrated on a chip, the memory 301, the processor 302 and the communication interface 303 can communicate with each other through an internal interface.

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

[0118] An embodiment of the present application also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned method of calibrating the millimeter-wave radar to sense the speed of the laser radar.

[0119] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" 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 can be combined in any one or N embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0120] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of technical features indicated. Thus, a feature specified as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of this application, "N" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0121] 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 more 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 be performed out of 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 those skilled in the art to which the embodiments of the present application belong.

[0122] It should be understood that various parts of the present application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiment, the N steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array, a field programmable gate array, etc.

[0123] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related 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 embodiment.

[0124] 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 limitations on the present application. Ordinary technicians in this field can change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A method for calibrating laser radar sensing speed using millimeter wave radar, characterized in that: The following steps are involved: Establish a motion state model of the target to be observed; Obtaining laser radar observation data and millimeter wave radar observation data of the target to be observed, and using the motion state model to predict a state estimate of the target to be observed at the next moment; Using a first preset Kalman filter, the state estimate is corrected based on the lidar observation data to obtain a corrected state estimate, and using a second preset Kalman filter, the corrected state estimate is optimized based on the millimeter-wave radar observation data to obtain a target state estimate.

2. The method according to claim 1, characterized in that The motion state model is: X k =F k-1 x k-1 +w k-1 ; Among them, x k is the state equation at time k, F k-1 is the state transfer matrix at time k-1, x k-1 is the state equation at time k-1, w k-1 is the process noise at time k-1.

3. The method according to claim 1, characterized in that The laser radar observation data is: in, is the observation value of the laser radar at time k, H LiDAR is the observation matrix of the lidar, is the observation noise of the lidar at time k.

4. The method according to claim 1, wherein The first preset Kalman filter is: K LiDAR =P k|k-1 (H LiDAR ) T (H LiDAR P k|k-1 (H LiDAR ) T +R LiDAR ) -1 ; Among them, K LiDAR is the first Kalman gain, P k|k-1 is the prediction covariance matrix, R LiDAR is the observation noise of the lidar.

5. The method according to claim 1, characterized in that The second preset Kalman filter is: Among them, K Radar is the second Kalman gain, is the updated state covariance matrix of the lidar, H Radar is the millimeter wave radar observation matrix, R Radar is the observation noise of millimeter-wave radar.

6. A device for calibrating laser radar sensing speed using millimeter wave radar, characterized in that: include: Establishing a module for establishing a motion state model of the target to be observed; An acquisition module, configured to acquire the laser radar observation data and the millimeter wave radar observation data of the target to be observed, and predict the state estimation of the target to be observed at the next moment using the motion state model; An estimation module is used to use a first preset Kalman filter to correct the state estimate based on the lidar observation data to obtain a corrected state estimate, and to use a second preset Kalman filter to optimize the corrected state estimate based on the millimeter-wave radar observation data to obtain a target state estimate.

7. The device according to claim 6, characterized in that The motion state model is: x k =F k-1 x k-1 +w k-1 ; Among them, x k is the state equation at time k, F k-1 is the state transfer matrix at time k-1, x k-1 is the state equation at time k-1, w k-1 is the process noise at time k-1.

8. The device according to claim 6, characterized in that The laser radar observation data is: in, is the observation value of the laser radar at time k, H LiDAR is the observation matrix of the lidar, is the observation noise of the lidar at time k.

9. An electronic device, 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 method for calibrating the laser radar sensing speed by a millimeter-wave radar 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 method for calibrating a millimeter-wave radar to sense laser radar speed as described in any one of claims 1 to 5.

Images