Autonomous vehicle scan matching and radar pose estimator based on super-local sub-graphs

Abstract

A scan matching and radar pose estimator for determining a final radar pose of an autonomous vehicle includes an autonomous driving controller instructed to determine a super-local subgraph based on a predetermined number of consecutive aggregated filtered data point cloud scans and associated pose estimates. The autonomous driving controller determines an initial estimated pose by way of aligning a most recently aggregated filtered data point cloud scan with a most recent super-local subgraph based on an iterative closest point (ICP) alignment algorithm. The autonomous driving controller determines a pose graph based on the most recent super-local subgraph and proximate radar point cloud scans and performs a multi-view non-linear ICP algorithm to adjust the initial estimated pose corresponding to the proximate radar point cloud scans in a moving window fashion to determine a locally adjusted pose.

Description

Technical Field

[0001] This disclosure relates to a system and method for scanning matching and radar attitude estimators for autonomous vehicles based on hyperlocal subgraphs, wherein the hyperlocal subgraph is based on a predetermined number of consecutive aggregated filtered data point cloud scans and their latest associated attitude estimates. Background Technology

[0002] Autonomous vehicles can use various onboard technologies and sensors to drive from a starting point to a predetermined destination with limited or no human intervention. Autonomous vehicles include various autonomous sensors, such as, but not limited to, cameras, radar, lidar, GPS, and inertial measurement units (IMUs) for detecting the vehicle's external environment and conditions. However, when an autonomous vehicle undergoes maintenance, or if an accident occurs while driving, or if it traverses obvious potholes or obstacles, if a camera or radar is removed from its mount, the camera or radar sensors need to be recalibrated—a manual and often cumbersome process. Furthermore, if the autonomous vehicle undergoes wheel alignment, the cameras and radar also require recalibration. This is because the vehicle's wheels determine its direction of travel, which affects the aiming of the cameras and radar.

[0003] Millimeter-wave (mmWave) radar is a specific technology that can be used in autonomous vehicles. For example, mmWave radar can be used for forward and rear collision warnings, adaptive cruise control and automatic parking, and autonomous driving on streets and highways. It should be recognized that an advantage of mmWave radar compared to other sensor systems is its ability to operate in most weather and low-light conditions. mmWave radar can measure the distance, angle, and Doppler (radial velocity) of moving objects. Radar point clouds (which can be used to determine the position, velocity, and trajectory of objects) can be determined from data acquired by mmWave radar based on various clustering and tracking algorithms. However, radar point clouds based on data acquired by mmWave radar, especially low-cost system-on-a-chip (SoC) based mmWave radar, can be too noisy and sparse to be used for the robust and accurate attitude estimation required for dynamic calibration purposes.

[0004] Therefore, although current radar attitude estimation methods for autonomous vehicles have achieved their intended purpose, there is still a need in the field for an improved method for vehicle radar attitude estimation. Summary of the Invention

[0005] According to several aspects, a scan matching and radar attitude estimator for determining the final radar attitude of an autonomous vehicle is disclosed. The scan matching and radar attitude estimator includes an autonomous driving controller instructed to determine a hyperlocal subgraph based on a predetermined number of consecutively aggregated filtered point cloud scans and associated attitude estimates. The predetermined number of consecutively aggregated filtered point cloud scans are aggregated filtered point cloud scans determined based on data acquired by individual radar sensors mounted on the autonomous vehicle. The autonomous driving controller is instructed to align the latest aggregated filtered point cloud scan with the nearest hyperlocal subgraph based on an Iterative Nearest Point (ICP) alignment algorithm to determine an initial estimated attitude. The autonomous driving controller is instructed to determine an attitude map based on the nearest hyperlocal subgraph and neighboring radar point cloud scans. The autonomous driving controller is instructed to execute a multi-view nonlinear ICP algorithm to adjust the initial estimated attitude corresponding to the neighboring radar point cloud scans in a moving window manner to determine a locally adjusted attitude. The locally adjusted attitude is the final radar attitude.

[0006] In one aspect, the autonomous driving controller executes instructions to perform a loop detection algorithm to detect that the autonomous vehicle is currently at a previously visited location, wherein a loop is detected while the autonomous vehicle is currently at a previously visited location.

[0007] In another aspect, in response to the detection loop, the autonomous driving controller is instructed to execute a nonlinear optimization routine to perform global attitude adjustment on the loop closure attitude map to determine the loop-adjusted radar attitude, and set the loop-adjusted radar attitude as the final radar attitude.

[0008] In another aspect, the nearest hyperlocal subgraph is a fixed constraint, and the neighboring radar point cloud scans are adjusted relative to each other and relative to the nearest hyperlocal subgraph.

[0009] In one aspect, edge constraints are determined between each neighboring radar point cloud scan.

[0010] In another aspect, edge constraints are determined between each of the neighboring radar point cloud scans and the nearest hyperlocal subgraph.

[0011] In another aspect, the autonomous driving controller determines the initial estimated pose by adjusting the maximum distance threshold parameter of the ICP alignment algorithm in order to determine the correspondence between the aggregated filtered data point cloud scan and the points located on the nearest hyperlocal subgraph.

[0012] In one aspect, a predetermined number of aggregated filtered data point cloud scans and associated attitude estimations depend on the sampling rate of a single radar sensor.

[0013] In another aspect, an initial pose estimate is used instead of the associated pose estimate to determine the hyperlocal subgraph, which is based on a predetermined number of aggregated filtered point cloud scans.

[0014] In another aspect, locally adjusted poses are used instead of associated pose estimates to determine hyperlocal subgraphs, the associated pose estimates being based on a predetermined number of aggregated filtered point cloud scans.

[0015] In one aspect, the autonomous driving controller is instructed to determine the predicted attitude by determining a relative transformation representing the relative motion between the last two consecutive attitude estimates associated with the aggregated filtered data point cloud scan, converting the relative transformation of the relative motion between the last two consecutive attitude estimates into velocity based on the time difference between the last attitude estimate and the current attitude estimate, and converting the relative transformation from velocity into position based on the time difference between the last attitude estimate and the current attitude estimate. The difference in position between the last attitude estimate and the current attitude estimate is the predicted attitude.

[0016] In one aspect, a method for determining the final radar attitude of an autonomous vehicle is provided. The method includes determining a hyperlocal subgraph based on a predetermined number of consecutively aggregated filtered point cloud scans and associated attitude estimates. The predetermined number of consecutively aggregated filtered point cloud scans are based on an aggregated filtered point cloud scan, and this aggregated filtered point cloud scan is determined based on data acquired by individual radar sensors mounted on the autonomous vehicle. The method includes determining an initial estimated attitude by aligning the most recently aggregated filtered point cloud scan with the nearest hyperlocal subgraph using an ICP alignment algorithm. The method also includes determining an attitude map based on the nearest hyperlocal subgraph and neighboring radar point cloud scans. Finally, the method further includes performing a multi-view nonlinear ICP algorithm to adjust the initial estimated attitude corresponding to the neighboring radar point cloud scans in a moving window manner to determine a locally adjusted attitude, wherein the locally adjusted attitude is the final radar attitude.

[0017] In another aspect, the method includes performing a loop detection algorithm to detect that the autonomous vehicle is currently at a previously visited location, wherein a loop is detected while the autonomous vehicle is currently at a previously visited location.

[0018] In another aspect, in response to a detection loop, the method includes executing a nonlinear optimization routine to perform a global attitude adjustment on the loop closure attitude map to determine the loop-adjusted radar attitude, and setting the loop-adjusted radar attitude as the final radar attitude.

[0019] In one aspect, the method includes determining edge constraints between each neighboring radar point scan.

[0020] In another aspect, the method includes determining edge constraints between each of the neighboring radar point cloud scans and the nearest hyperlocal subgraph.

[0021] In another aspect, the method includes determining the initial estimated pose by adjusting the maximum distance threshold parameter of the ICP alignment algorithm to determine the correspondence between the aggregated filtered data point cloud scan and the points located on the nearest hyperlocal subgraph.

[0022] In one aspect, the method includes using an initial pose estimate instead of an associated pose estimate to determine a hyperlocal subgraph, the associated pose estimate being based on a predetermined number of aggregated filtered point cloud scans.

[0023] In one aspect, the method includes using an initial pose estimate instead of an associated pose estimate to determine a hyperlocal subgraph, the associated pose estimate being based on a predetermined number of aggregated filtered point cloud scans.

[0024] In another aspect, a scan matching and radar attitude estimator for determining the final radar attitude of an autonomous vehicle is disclosed. The scan matching and radar attitude estimator includes an autonomous driving controller instructed to determine a hyperlocal subgraph based on a predetermined number of consecutively aggregated filtered point cloud scans and associated attitude estimates. The predetermined number of consecutively aggregated filtered point cloud scans are based on data acquired by a single radar sensor mounted on the autonomous vehicle. The autonomous driving controller is instructed to determine an initial estimated attitude by aligning the most recently aggregated filtered point cloud scan with the most recent hyperlocal subgraph using an ICP alignment algorithm. The autonomous driving controller is instructed to determine an attitude map based on the most recent hyperlocal subgraph and neighboring radar point cloud scans. The autonomous driving controller is instructed to perform a multi-view nonlinear ICP algorithm to adjust the initial estimated attitude corresponding to the neighboring radar point cloud scans in a moving window manner to determine a locally adjusted attitude. In one aspect, the autonomous driving controller is instructed to perform a loop detection algorithm to detect that the autonomous vehicle is currently at a previously visited location, wherein loops are detected when the autonomous vehicle is currently at a previously visited location. In response to the detection loop, the autopilot controller executes a nonlinear optimization routine to perform global attitude adjustment on the loop-closed attitude map to determine the loop-adjusted radar attitude. The autopilot controller is then instructed to set the loop-adjusted radar attitude as the final radar attitude.

[0025] Other applicable scope will become apparent from the detailed description provided below. It should be understood that the description and specific embodiments are for illustrative purposes only and are not intended to limit the scope of this disclosure. Attached Figure Description

[0026] The accompanying drawings described herein are for illustrative purposes only and are not intended to limit the scope of this disclosure in any way.

[0027] Figure 1 This is a schematic diagram of an autonomous vehicle including multiple radar sensors and an autonomous driving controller according to an exemplary embodiment, wherein the autonomous driving controller includes an attitude estimation pipeline for determining calibration coordinates;

[0028] Figure 2 This is an illustration based on an exemplary embodiment. Figure 1 The block diagram shows a portion of the attitude estimation pipeline, including the scan matching and radar attitude estimator module.

[0029] Figure 3 It is an attitude map including a fixed hyperlocal subgraph and multiple neighboring radar point scans according to an exemplary embodiment;

[0030] Figure 4 This is an illustration of an exemplary embodiment for use with Figure 2 The flowchart shows the method by which the scan matching and radar attitude estimator module 46 determines the final radar attitude. Detailed Implementation

[0031] The following description is merely exemplary in nature and is not intended to limit this disclosure, its application, or its uses.

[0032] refer to Figure 1 An exemplary autonomous vehicle 10 is shown. The autonomous vehicle 10 has an autonomous driving system 12, which includes an autonomous driving controller 20 that electronically communicates with multiple onboard autonomous sensors 22 and multiple vehicle systems 24. In such a way... Figure 1 In the example shown, the multiple onboard autonomous sensors 22 include one or more radar sensors 30, one or more cameras 32, an inertial measurement unit (IMU) 34, a global positioning system (GPS) 36, and a lidar 38; however, it should be recognized that additional sensors may also be used. The multiple radar sensors 30 may be mounted on the front 14, rear 16, and / or sides 18 of the autonomous vehicle 10 to detect objects in the environment surrounding the autonomous vehicle 10. Each radar sensor 30 performs multiple individual scans of the environment surrounding the autonomous vehicle 10 to obtain data in the form of a radar point cloud scan including multiple detection points.

[0033] The autonomous driving controller 20 includes an attitude estimation pipeline 40, comprising a scan aggregator and filter 42, an inertial navigation system (INS) module 44, a scan matching and radar attitude estimator module 46, and a calibration module 48. As explained below, the scan aggregator and filter 42 determines an aggregated filtered data point cloud scan 50 based on data acquired by individual radar sensors 30 of the environment surrounding the autonomous vehicle 10. The aggregated filtered data point cloud scan 50 is sent to the scan matching and radar attitude estimator module 46. The scan timestamp associated with the aggregated filtered data point cloud scan 50 is sent to the INS module 44. The INS module 44 determines the IMU attitude 52 sent to the calibration module 48, and the scan matching and radar attitude estimator module 46 determines the final radar attitude 54 sent to the calibration module 48. The calibration module 48 determines six degrees of freedom (6DoF) variables 56, including x, y, and z coordinates and the roll φ, pitch θ, and yaw Ψ of the autonomous vehicle 10. In one embodiment, the six degrees of freedom (6DoF) variables 56 are transmitted via radar to vehicle calibration parameters used to automatically align the radar sensor 30 with the center of gravity G of the autonomous vehicle 10. However, it should be recognized that the disclosed scan aggregator and filter 42 and scan matching and radar attitude estimator module 46 are not limited to transmitting 6DoF variables via radar to vehicle calibration parameters and can also be used in other applications. For example, in another embodiment, the scan aggregator and filter 42 and scan matching and radar attitude estimator module 46 can be used in 3D radar-based simultaneous localization and mapping (SLAM) applications.

[0034] It should be recognized that the radar point cloud obtained by radar sensor 30 can be sparse and, in some instances, includes noise and jitter data, ghosting detection, reflections, and clutter. As explained below, despite the sparse radar point cloud, the disclosed scan matching and radar attitude estimator module 46 employs a hyperlocal subgraph to determine the final radar attitude 54 with improved accuracy and robustness. Specifically, as explained below, the hyperlocal subgraph is used for iterative nearest-point (ICP) scan matching, multi-view nonlinear ICP adjustment, and attitude map loop closure, which mitigates the impact of the sparse radar point cloud.

[0035] The autonomous vehicle 10 can be any type of vehicle, such as, but not limited to, a sedan, truck, SUV, minivan, or motorhome. In one non-limiting embodiment, the autonomous vehicle 10 is a fully autonomous vehicle, including an Automated Driving System (ADS) that performs all driving tasks. Alternatively, in another embodiment, the autonomous vehicle 10 is a semi-autonomous vehicle, including an Advanced Driver Assistance System (ADAS) for assisting the driver in steering, braking, and / or acceleration. The autonomous driving controller 20 determines autonomous driving characteristics, such as the perception, planning, localization, mapping, and control of the autonomous vehicle 10. Although Figure 1 The automated driving controller 20 is shown as a single controller, but it should be understood that multiple controllers may also be included. The multiple vehicle systems 24 include, but are not limited to, a braking system 70, a steering system 72, a powertrain system 74, and a suspension system 76. The automated driving controller 20 sends vehicle control commands to the multiple vehicle systems 24 to guide the automated driving vehicle 10.

[0036] Radar sensor 30 may be a short-range radar for detecting objects at a distance of approximately 1 meter to approximately 20 meters from the autonomous vehicle 10, a medium-range radar for detecting objects at a distance of approximately 1 meter to approximately 60 meters from the autonomous vehicle 10, or a long-range radar for detecting objects at a distance of up to approximately 260 meters from the autonomous vehicle 10. In one embodiment, one or more of radar sensors 30 include millimeter-wave (mmWave) radar sensors and have a limited field of view in a particular low-cost, system-on-a-chip (SoC) based millimeter-wave radar. In another embodiment, radar sensor 30 includes one or more radar sensors that rotate 360 ​​degrees.

[0037] For reference Figure 2 The diagram shows a block diagram of the scan matching and radar attitude estimator module 46, which includes an iterative nearest point (ICP) scan matching submodule 60, a multi-view nonlinear ICP adjustment submodule 62, a subgraph submodule 64, an attitude graph loop closure submodule 66, and one or more spatial databases 68. (The data comes from the scan aggregator and filter 42.) Figure 1The aggregated and filtered point cloud data 50 (visible in the image) is sent to each of the submodules 60, 62, 64, and 66 of the scan matching and radar attitude estimator module 46. The ICP scan matching submodule 60 determines the final estimated attitude 80 sent to the remaining submodules 62, 64, and 66. The multi-view nonlinear ICP adjustment submodule 62 determines the locally adjusted attitude 82 sent to the remaining submodules 60, 64, and 66. The subgraph submodule 64 determines the hyperlocal subgraph 84 sent to the remaining submodules 60, 62, and 66. Finally, if applicable, the attitude map loop closure submodule 66 determines the loop-adjusted radar attitude 86 sent to the remaining submodules 60, 62, and 64.

[0038] Also refer to Figure 1 and Figure 2 The final radar attitude 54 sent to the calibration module 48 is either a locally adjusted attitude 82 determined by the multi-view nonlinear ICP adjustment submodule 62, or a loop-adjusted radar attitude 86 determined by the attitude map loop closure submodule 66. Specifically, in the event that the attitude map loop closure submodule 66 detects a loop closure, the final radar attitude 54 is the loop-adjusted radar attitude 86; otherwise, the final radar attitude 54 is the locally adjusted attitude 82 determined by the multi-view nonlinear ICP adjustment submodule 62. As explained below, the locally adjusted attitude 82 and the loop-adjusted radar attitude 86 are based on a hyperlocal subgraph 84 determined by the subgraph submodule 64. Typically, the hyperlocal subgraph 84 is determined based on a predetermined number N of continuously aggregated filtered data point cloud scans 50 and associated pose estimations; however, in events where the available number of continuously aggregated filtered data point cloud scans 50 is less than the predetermined number N of continuously aggregated filtered data point cloud scans 50, the hyperlocal subgraph 84 may be determined based on the currently available number of continuously aggregated filtered data point cloud scans 50.

[0039] refer to Figure 2Subgraph submodule 64 determines a hyperlocal subgraph 84 based on a predetermined number N of continuously aggregated filtered data point cloud scans 50 and associated attitude estimates; or, in an alternative, if the available number of continuously aggregated filtered data point cloud scans 50 is less than the predetermined number N, the hyperlocal subgraph 84 may be determined based on the currently available number of continuously aggregated filtered data point cloud scans 50, wherein the aggregated filtered data point cloud scans 50 are determined based on data acquired by individual radar sensors 30. Subgraph submodule 64 initially determines the hyperlocal subgraph 84 based on the predetermined number N of aggregated filtered data point cloud scans 50 received by the self-scanning aggregator and filter 42 in conjunction with associated attitude estimates. Each historically aggregated filtered data point cloud scan 50 is stored in combination with an associated attitude estimate. It should be recognized that the most recent attitude estimate received from one of submodules 60, 62, and 66 can be used alternatively to determine the hyperlocal subgraph 84. Specifically, the initial estimated attitude 80 from the ICP scan matching submodule 60 takes precedence over the associated attitude estimate from the aggregated filtered data point cloud scan 50, the locally adjusted attitude 82 determined by the multi-view nonlinear ICP adjustment submodule 62 takes precedence over the initial estimated attitude 80, and the loop-adjusted radar attitude 86 determined by the attitude map loop closure submodule 66 takes precedence over the initial estimated attitude 80.

[0040] Hyperlocal subgraph 84 shows a graph constructed from a predetermined number N of continuously aggregated filtered data point cloud scans 50 and associated attitude estimates. The predetermined number N of aggregated filtered data point cloud scans 50 and associated attitude estimates depend on the radar sensor 30 ( Figure 1 The sampling rate of the radar sensor 30 is 10. In one embodiment, the predetermined number N is half the sampling rate of the radar sensor 30. For example, in one embodiment, the sampling rate of the radar sensor 30 is approximately 20 frames per second (fps), therefore, the predetermined number N is 10.

[0041] The ICP scan matching submodule 60 of the scan matching and radar attitude estimator module 46 is derived from the scan aggregator and filter 42. Figure 1 The system receives the latest aggregated filtered point cloud scan 50 and the nearest hyperlocal subgraph 84 from the subgraph submodule 64 as input, and determines the initial estimated pose 80 of the latest aggregated filtered point cloud scan 50. In the event that the number of available aggregated filtered point cloud scans 50 is less than a predetermined number N, the nearest hyperlocal subgraph 84 can be determined based on the available number of aggregated filtered point cloud scans 50.

[0042] Because of the inconsistent arrival times between scans, it should be recognized that directly using the relative pose change between two consecutive scans to predict the initial estimated pose 80 leads to inaccuracies and convergence problems associated with ICP scan matching. Therefore, the ICP scan matching submodule 60 determines the initial estimated pose 80 by first determining the predicted pose, which will be described in more detail below. It should be recognized that the predicted pose is the internal predicted pose of the ICP scan matching submodule 60. Then, the ICP scan matching submodule 60 adjusts the maximum distance threshold parameter of the ICP alignment algorithm to determine the correspondence between the aggregated filtered data point cloud scan 50 and the nearest hyperlocal subgraph 84. It should be recognized that this is based on the autonomous vehicle 10 ( Figure 1 The maximum distance threshold is adjusted by both linear velocity and angular velocity. Finally, the newly aggregated filtered data point cloud scan 50 is aligned with the nearest hyperlocal sub-map 84 by an ICP alignment algorithm, and the ICP scan matching submodule 60 determines the initial estimated attitude 80, which is used as the starting point, to determine the final radar attitude 54.

[0043] In one embodiment, the ICP scan matching submodule 60 determines the predicted attitude by first determining the relative transformation showing the relative motion between the last two consecutive attitude estimates associated with the latest aggregated filtered data point cloud scan 50. Then, based on the time difference between the last two consecutive attitude estimates associated with the latest radar point scan, the relative transformation between the last two consecutive attitude estimates associated with the latest aggregated filtered data point cloud scan 50 is converted to velocity. Specifically, based on the Lie algebra logarithmic function, the relative transformation is converted to the se(3) space using the time difference between the last two consecutive attitude estimates associated with the latest aggregated filtered data point cloud scan 50. Then, based on the time difference between the last attitude estimate and the current attitude estimate, the relative transformation is converted from velocity to position, where the difference in position between the last attitude estimate and the current attitude estimate is the predicted attitude estimate. Specifically, based on the Lie algebra exponential function, the relative transformation is converted to the special Euclidean group SE(3) space using the time difference between the last attitude estimate and the current attitude estimate, where se(3) is the tangent space of the special Euclidean group SE(3).

[0044] Also refer to Figure 2 and Figure 3 The multi-view nonlinear ICP adjustment submodule 62 determines the attitude map 100, which includes the nearest hyperlocal submap 84 and the neighboring radar point cloud scan 104 (see [link]). Figure 3The multi-view nonlinear ICP adjustment submodule 62 aligns multiple neighboring radar point cloud scans 104 simultaneously with each other and with the nearest hyperlocal submap 84. It should be understood that the nearest hyperlocal submap is a fixed constraint; however, the neighboring radar point cloud scans 104 are adjusted relative to each other and with respect to the nearest hyperlocal submap 84.

[0045] refer to Figure 3 The most recent hyperlocal sub-map 84 is constructed from a predetermined number N of continuously aggregated filtered data point cloud scans 50 and the corresponding initial estimated attitude 80 corresponding to the latest point cloud scan S11 determined by the ICP scan matching submodule 60. It should be understood that the corresponding final radar attitude 54 has already been stored in memory for previous radar point cloud scans S1-S10. In such cases... Figure 3 In the example shown, the predetermined number N is 10, and the nearest hyperlocal subgraph 84 includes 10 consecutive radar point cloud scans, S1-S10. Attitude map 100 also includes N+1 neighboring radar point cloud scans 104. In other words, attitude map 100 includes one additional neighboring radar point cloud scan 104 compared to the nearest hyperlocal subgraph 84. Figure 3 In the example shown, attitude diagram 100 includes neighboring radar point cloud scans S1, S2, S3, S4, S5, S6, S7, S8, S9, S10, and S11, where S11 represents the latest radar point cloud scan from radar sensor 30. Figure 1 It should be recognized that the proximity is measured based on the latest attitude estimate corresponding to the nearby radar point cloud scan 104.

[0046] Also refer to Figure 2 and Figure 3 Based on the corresponding initial estimated attitude 80 of the neighboring radar point cloud scans 104, the multi-view nonlinear ICP adjustment submodule 62 determines the attitude map 100 by initially determining the neighboring radar point cloud scans 104. The multi-view nonlinear ICP adjustment submodule 62 also determines the proximity between the neighboring radar point cloud scans 104 based on the K-nearest neighbor (kNN) technique. Specifically, the kNN technique can be used to determine the neighborhood matrix or adjacency matrix between the neighboring radar point cloud scans 104.

[0047] Then, the multi-view nonlinear ICP adjustment submodule 62 determines the attitude map 100 by calculating the point-to-point correspondence between each of the neighboring radar point cloud scans 104 and the nearest hyperlocal subgraph 84 based on a maximum corresponding distance threshold. In a non-limiting embodiment, the maximum corresponding distance threshold is approximately 1.5 meters; however, it should be understood that the maximum corresponding distance threshold is based on the autonomous vehicle 10 ( Figure 1The linear velocity and angular velocity are both adjusted. The multi-view nonlinear ICP adjustment submodule 62 determines the edge constraints 108, used to connect nodes representing neighboring radar point cloud scans 104. Edge constraints 108 define the relative attitude between the nodes and the measurement uncertainty. In... Figure 3 In the example shown, variable k equals 3. Therefore, three edge constraints 108 are determined between their respective neighboring radar point cloud scans 104 and one of the remaining neighboring radar point cloud scans 104. Edge constraints 110 are also determined between each of the neighboring radar point cloud scans 104 and the nearest hyperlocal subgraph 84. In other words, the nearest hyperlocal subgraph 84 is adjacent to each of the neighboring radar point cloud scans 104. The multi-view nonlinear ICP adjustment submodule 62 then assigns weights to each of the edge constraints 108, 110, where the weights are measurements of the distances between the respective point correspondences found between any two neighboring radar point cloud scans 104 or between a neighboring radar point cloud scan 104 and the nearest hyperlocal subgraph 84. In one embodiment, the distance measurement is the average of the median of all corresponding distances and the maximum corresponding distance. It should be understood that the nearest hyperlocal subgraph is a point cloud comprising multiple scans.

[0048] Once the attitude map 100 is constructed, the multi-view nonlinear ICP adjustment submodule 62 executes a multi-view nonlinear ICP algorithm to adjust the initial estimated attitude 80 corresponding to neighboring radar point cloud scans 104 in a moving window manner to determine the locally adjusted attitude 82. It should be understood that the moving window is a global window that moves across N+1 last scans (including the last scan), and is incremented by a single radar point cloud scan 104 each time a new scan is introduced. One example of a multi-view nonlinear ICP algorithm that can be used is the Levonburg-Marquardt ICP; however, it should be understood that other methods can also be used. Once the multi-view nonlinear ICP adjustment submodule 62 executes the multi-view nonlinear ICP adjustment algorithm to adjust the initial estimated attitude 80 corresponding to neighboring radar point cloud scans 104, only the initial estimated attitude 80 associated with the oldest scan S1 of the N+1 scans is ultimately determined and maintained as its locally adjusted attitude. It should be understood that the oldest scan S1 is skipped during the next moving window of the multi-view nonlinear ICP algorithm.

[0049] Refer again Figure 2 By first executing the loop detection algorithm, the attitude map loop closure submodule 66 of the scanning matching and radar attitude estimator module 46 determines the radar attitude 86 of the loop adjustment, so as to determine the autonomous vehicle 10 ( Figure 1The system checks whether the vehicle is currently in a previously visited location. When the autonomous vehicle 10 is currently in a previously visited location, this is considered a detection loop. In response to a detection loop, the attitude graph loop closure submodule 66 executes a nonlinear optimization routine to determine the radar attitude 86 for loop adjustment. An example of the nonlinear optimization routine used to determine the radar attitude 86 for loop adjustment is a nonlinear least squares routine, such as, but not limited to, the Levenberg-Marquardt algorithm.

[0050] As in Figure 2 As seen, the attitude map loop closure submodule 66 communicates with one or more spatial databases 68. With the autonomous vehicle 10 ( Figure 1 As the hyperlocal subgraph 84 progresses, the attitude graph loop closure submodule 66 determines the two-dimensional feature descriptor of the hyperlocal subgraph 84 determined by the subgraph submodule 64 (stored in one or more spatial databases 68). For example, the hyperlocal subgraph 84 has multiple views. Figure 2 2D projection (M2DP) feature descriptors are stored in one or more spatial databases 68. An example of a spatial database 68 that can be used is a kd-tree, a spatial partitioning data structure for organizing points in k-dimensional space. In this embodiment, only the 2D feature descriptors of preselected or key hyperlocal subgraphs 84 can be stored in one or more spatial databases 68, wherein the key spatial database 68 is defined based on threshold motion amounts occurring between consecutive hyperlocal subgraphs 84.

[0051] With autonomous vehicles 10 ( Figure 1 During the movement of the ICP scan matching submodule 60, the scan aggregator and filter 42 receive data from the scan aggregator and filter 42. Figure 1The final data point cloud scan is performed. As the ICP scan matching submodule 60 processes the final data point cloud scan, the attitude map loop closure submodule 66 determines the two-dimensional feature descriptor of the hyperlocal subgraph 84 corresponding to the final filtered data point cloud scan 50, and finds matching two-dimensional feature descriptors stored in one or more spatial databases 68. In response to determining that the matching two-dimensional feature descriptor stored in one or more spatial databases 68 is within a predetermined threshold, the loop closure ICP scan matching algorithm is executed. When attempting to find a match, the closest match among all candidate matches within one or more spatial databases 68 within the predetermined threshold is selected. Specifically, the loop closure ICP scan matching algorithm is performed between the final data point cloud scan and the scan corresponding to the matching two-dimensional feature descriptor stored in one or more spatial databases 68, based on a relaxed corresponding distance threshold. The predetermined threshold is determined empirically, and in one embodiment, when M2DP is used, the predetermined threshold is 0.35 (the predetermined threshold has no units). The relaxed corresponding distance threshold is also determined empirically and depends on the configuration. In one embodiment, the relaxed corresponding distance threshold is approximately 5 meters.

[0052] In response to the determination that the loop closure overlap score and root mean square error (RMSE) fall within a predefined threshold, the last radar point cloud scan and the corresponding loop closure transformation are added to the loop closure attitude map. In one embodiment, the loop closure overlap score is at least 0.75 (at least 75% overlap), and the RMSE is approximately 3 meters. The loop closure attitude map is constructed by the attitude map loop closure submodule 66, which is built from the aggregated filtered data point scans 50 and their associated attitudes, and the edge constraints of the loop closure attitude map are based on relative motion constraints. The attitude map loop closure submodule 66 performs a nonlinear optimization routine to perform global attitude adjustment on the loop closure attitude map to determine the loop-adjusted radar attitude 86. As described above, an example of the nonlinear optimization routine used to determine the loop-adjusted radar attitude 86 is a nonlinear least squares routine, such as, but not limited to, the Levenberg-Marquardt algorithm.

[0053] Figure 4 This illustrates the use of scan matching and radar attitude estimator module 46 ( Figure 2 ) Determine the final radar attitude 54 ( Figure 2 Flowchart of the process. General reference. Figure 1-4Method 200 begins at block 202. In block 202, the subgraph submodule 64 of the scan matching and radar attitude estimator module 46 initially determines a hyperlocal subgraph 84 based on a predetermined number N of aggregated filtered data point cloud scans 50 received by the self-scan aggregator and filter 42 in conjunction with the associated attitude estimation. Alternatively, if the available number of consecutively aggregated filtered data point cloud scans 50 is less than the predetermined number N, the most recent hyperlocal subgraph 84 can be determined based on the currently available number of aggregated filtered data point cloud scans 50. As described above, the subgraph submodule 64 initially determines the hyperlocal subgraph 84 based on the predetermined number N of aggregated filtered data point cloud scans 50 and the associated attitude estimation (received by the self-scan aggregator and filter 42); however, it should be recognized that the subgraph submodule 64 utilizes the most recent attitude estimation received by one of the submodules 60, 62, and 66. Method 200 can then proceed to block 204.

[0054] In box 204, the newly aggregated filtered data point cloud scan 50 is aligned with the nearest hyperlocal sub-image 84 using an ICP alignment algorithm. The ICP scan matching sub-module 60 of the scan matching and radar attitude estimator module 46 determines the initial estimated attitude 80. Specifically, the ICP scan matching sub-module 60 determines the pre-attitude using the contours in sub-boxes 204A-204C.

[0055] In sub-block 204A, the ICP scan matching submodule 60 determines a relative transformation representing the relative motion between the last two consecutive attitude estimates associated with the latest aggregated filtered data point cloud scan 50. Method 200 then proceeds to sub-block 204B. In sub-block 204B, the relative transformation between the last two consecutive attitude estimates associated with the latest aggregated filtered data point cloud scan 50 is then converted to velocity based on the time difference between the last two consecutive attitude estimates associated with the latest radar point scan. In sub-block 204C, the relative transformation is converted from velocity to position based on the time difference between the last attitude estimate and the current attitude estimate, where the difference in position between the last attitude estimate and the current attitude estimate is the predicted attitude. Method 200 then proceeds to block 206.

[0056] In box 206, the attitude map 100 is determined for the first time based on the nearest hyperlocal subgraph 84 and the neighboring radar point cloud scan 104 (see [link]). Figure 3The multi-view nonlinear ICP adjustment submodule 62 determines the local adjustment attitude 82 and then executes the multi-view nonlinear ICP algorithm to adjust the initial estimated attitude 80 corresponding to the neighboring radar point cloud scan 104 in a moving window manner to determine the local adjustment attitude 82. In one embodiment, the attitude map loop closure module 66 does not detect loop closure. Therefore, the final radar attitude 54 is the local adjustment attitude 82. Then, method 200 may end or return to block 202. However, in the event that loop closure is detected by the attitude map loop closure block 66, method 200 may enter block 208.

[0057] In block 208, the attitude map loop closure block 66 executes a loop detection algorithm to detect whether the autonomous vehicle 10 is currently at a previously visited location. As described above, a loop is detected when the autonomous vehicle 10 is currently at a previously visited location. Then, method 200 can proceed to block 210.

[0058] In block 210, in response to the detection loop, the attitude map loop closure submodule 66 executes a nonlinear optimization routine to perform a global attitude adjustment on the loop-closed attitude map, thereby determining the loop-adjusted radar attitude 86. As described above, an example of the nonlinear optimization routine used to determine the loop-adjusted radar attitude 86 is a nonlinear least squares routine, such as, but not limited to, the Levenberg-Marquardt algorithm. It should be recognized that the final radar attitude 54 is the loop-adjusted radar attitude 86. Method 200 can then conclude.

[0059] Referring generally to the accompanying drawings, the disclosed scan matching and radar attitude estimator for autonomous vehicles offers various technical effects and benefits. Specifically, this disclosure provides a scan matching and radar attitude estimator module that employs a hyperlocal subgraph to determine an enhanced accuracy final radar attitude if the radar point cloud acquired by the radar sensors of the autonomous vehicle is sparse. Specifically, the disclosed scan matching and radar attitude estimator employs a hyperlocal subgraph, which reduces the impact of sparse radar point clouds.

[0060] A controller can refer to electronic circuitry, combinational logic circuitry, a field-programmable gate array (FPGA), a processor (shared, dedicated, or grouped) that executes code, or a combination of some or all of the above, such as a system-on-a-chip, or a portion thereof. Furthermore, the controller can be microprocessor-based (such as a computer having at least one processor, memory (RAM and / or ROM), and associated input and output buses). The processor can operate under the control of an operating system residing in memory. The operating system can manage computer resources so that computer program code, presented as one or more computer software applications (such as application programs residing in memory), can have instructions that are executed by the processor. In an alternative embodiment, the processor can directly execute the application program, in which case the operating system can be omitted.

[0061] The descriptions in this disclosure are merely exemplary in nature, and variations that do not depart from the essential points of this disclosure are intended to be included within its scope. These variations are not considered to be outside the scope and scheme of this disclosure.

Claims

1. An apparatus for an autonomous vehicle, the apparatus comprising a scanning aggregator and a filter, The scanning aggregator and filter include: Multiple radar sensors are installed on the autonomous vehicle, each of which performs multiple individual scans of the surrounding environment to obtain data in the form of a radar point cloud including multiple detection points; and An autonomous driving controller, which electronically communicates with the plurality of radar sensors, wherein the autonomous driving controller is instructed to: Filter each of the individual scans to define the spatial region of interest; Each of the individual scans is filtered to remove detection points in the radar point cloud representing moving objects based on a first outlier-robust model estimation algorithm; Motion-compensated aggregation technology is used to aggregate a predetermined number of individual scans to generate aggregated data scans; and Multiple density-based clustering algorithms are applied to filter the aggregated data scans to determine the filtered aggregated data scans, which are used to construct hyperlocal subgraphs. The latest filtered aggregated data scan and the most recent hyperlocal subgraph are used as inputs to scan matching and radar attitude estimators.

2. The apparatus according to claim 1, wherein, The autonomous driving controller executes instructions to: Each of the individual scans is filtered based on the radar cross-sectional area value, wherein multiple monitoring points of the aforementioned radar point cloud representing the target object of the threshold size are retained.

3. The apparatus according to claim 1, wherein, The autonomous driving controller executes instructions to: The filtered aggregated data scan is filtered based on the second outlier-robust model estimation algorithm to determine the aggregated filtered data point cloud.

4. The apparatus according to claim 1, wherein, The first outlier-robust model estimation algorithm is either the Random Sampling Consensus (RANSAC) algorithm or the Random Sampling Maximum Likelihood Estimation (MLESAC) algorithm.

5. The apparatus according to claim 1, wherein, The predetermined number of individual scans depends on the sampling rate of the radar sensor.

6. The apparatus according to claim 1, wherein, The predetermined number of individual scans is greater than or equal to 3.

7. The apparatus according to claim 1, wherein, The motion-compensated scanning aggregation technology is motion-compensated RANSAC technology.

8. The apparatus according to claim 1, wherein, The multiple density-based clustering algorithms include filtering the aggregated data scans based on radar cross-sectional area values.

9. The apparatus according to claim 1, wherein, The multiple density-based clustering algorithms include filtering the aggregated data scan based on the distance to their respective neighboring detection points.

10. The apparatus according to claim 1, wherein, The aforementioned density-based clustering algorithms include density-based noisy spatial clustering (DBSCAN).

Images