2D Ego-Motion with Yaw Estimation using Only
mmWave Radars via Two-Way weighted ICP

Abstract

The interest in single-chip mmWave Radar is driven by their compact form factor, cost-effectiveness, and robustness under harsh environmental conditions. Despite its promising attributes, the principal limitation of mmWave radar lies in its capacity for autonomous yaw rate estimation. Conventional solutions have often resorted to integrating IMU (IMU) or deploying multiple radar units to circumvent this shortcoming. This paper introduces an innovative methodology for two-dimensional ego-motion estimation, focusing on yaw rate deduction, utilizing solely mmWave radar sensors. By applying a weighted ICP (ICP) algorithm to register processed points derived from heatmap data, our method facilitates 2D ego-motion estimation devoid of prior information. Through experimental validation, we verified the effectiveness and promise of our technique for ego-motion estimation using exclusively radar data.

I Introduction

Radar technology emerges as a promising solution for perception in robotics and autonomous systems due to its potential to overcome the limitations of traditional sensors like LiDAR and cameras. LiDAR systems are often hampered by their substantial size and associated costs, and cameras struggle with fluctuations in lighting conditions. In contrast, millimeter-wave radar emerges as a compact and economically feasible alternative, distinguished by its proficiency in estimating ego-motion within robotic frameworks in various environments. Despite the potential of radar, challenges persist in fully exploiting its capabilities. mmWave radar generates cube type data via the FFT (FFT) of chirp signals. Although there has been notable advancement in processing these signals for analysis, a discernible gap exists in the methodologies to leverage the lower-level radar data. Current approaches frequently employ radar in synergy with additional sensors, such as IMU or LiDAR systems, to estimate ego-motion.[1] This strategy primarily stems from the challenges associated with processing the high-level 4D point cloud outputs derived from radar data.

This paper proposes a novel Two-Way weighted ICP approach exclusively using mmWave radar for 2D ego-motion estimation, with a specific emphasis on yaw estimation. By eliminating the need for additional sensors, this research aims to overcome the limitations associated with traditional sensor fusion approaches. Our approach aims to enhance perception capabilities, offering advantages in cost-effectiveness, compactness, and resilience to environmental conditions. Through verification with publicly available datasets, we demonstrate the efficacy and potential of radar for perception tasks in real-world environments.

Figure 1: Visualization of the 2D ego-motion estimation process using only mmWave radar, along with the estimated trajectory in an indoor environment.

• Non-learning radar-sole 2D ego-motion estimation This paper presents the implementation of 2D ego-motion estimation, including rotation, solely utilizing mmWave radar data without integrating other sensors or needing a GPU.

• Clutter matching via feature sampling and two-way weighted ICP We effectively managed to match clutter mmWave radar data by employing feature sampling and a two-way weighted ICP approach.

• Validation of radar-only planar odometry The validity of our pipeline was verified through radar-only planar odometry performed on a public dataset.

II related work

Figure 2: Overview of the proposed method to estimate 2D ego-motion divided by two sections.

There are two type of mmWave radar, mechanically scanning radar and single-chip radar. Scanning radar provide high angular resolution images, facilitating point matching and widely applied in odometry or place recognition. However, bulky size, high-cost and high-power consumption makes unsuitable to the small mobile robots. Conversely, single-chip radar boasts a low cost and compact design, garnering increasing interest recently. Single-chip radar offers 4D point clouds (range, azimuth, elevation, and Doppler velocity), extracted from the radar data cube through signal processing, as well as heatmaps generated by applying FFT to the data cube. We analyzed methods utilizing single-chip radar, particularly focusing on ego-motion estimation.

II-A 4D Point Cloud based Ego-Motion Estimation

Most of the studies to estimate ego-motion by using radar utilized point clouds with doppler velocity from radar. First, by using single-chip radar solely, the linear velocity can be estimated by analyzing doppler velocities over azimuth angles.[2] However, since the doppler velcities are radial direction, to estimate yaw rate as well, using multiple radar sensors in [3], or assuming an Ackerman model are proposed in [2]. In the other side, inertial sensors are usually combined with radar to enhance the performance. In [4] proposed Extended Kalman Filter to use IMU and ego-velocity from radar, and [5][6] use deep learning networks by combining IMU.

II-B Heatmap based Ego-Motion Estimation

Relatively, there are few heatmap based research, the interest become rising recently. Due to the large number of unprocessed points in the heatmap, learning-based ego-motion estimation is a commom method. In [7], a supervised learning method was trained to estimate ego-motion by 3D intensity heatmap with IMU data, but its accuracy for the trajectory has not been verified. In [8], a deep network was trained to estimate the position directly from the heatmap. However, the development of heatmap-based method has mainly been focused on learning-based approaches, rather than being addressed by intuitively matching techniques.

III Method

To utilize the information from the low-level heatmap of the mmWave radar, we employed both single-chip radar and cascade radar to estimate the 2D ego-motion. While both configurations yield a heatmap, the cascade radar is preferred for estimating yaw rotation because it can generate a comprehensive range-elevation-azimuth intensity heatmap. Conversely, the single-chip radar, characterized by its 4D point cloud encompassing Doppler velocity, is utilized to estimate 2D linear velocity. Consequently, the methodology for estimating 2D ego-motion was structured into two distinct phases: linear velocity and yaw rate estimation, as depicted in Fig. 2.

III-A 2D Linear Velocity Estimation

First, we estimated the 2D linear velocity with the doppler velocity obtained from targets detected by single-chip radar, which introduced in [2]. Since the doppler velocity represents the velocity of objects relative to the radar, the radial velocity with respect to the azimuth $\theta$ follows the sinusoidal curve:

$$\begin{bmatrix}v_{r,1}\\ \vdots\\ v_{r,n}\end{bmatrix}=\begin{bmatrix}\cos\theta_{1}&\sin\theta_{1}\\ \vdots&\vdots\\ \cos\theta_{n}&\sin\theta_{n}\end{bmatrix}\begin{bmatrix}v_{x}\\ v_{y}\end{bmatrix}$$ (1)

with $v_{r}=\frac{\sqrt{x^{2}+y^{2}}}{r}v_{d}$, the radial velocity projected to the xy plane.

Generally, most points detected are static, facilitating the robust estimation of 2D linear velocity by applying a sine curve fitting to the data utilizing the RANSAC (RANSAC). Upon identifying inlier static objects, the computation of 2D linear velocity is refined to a least squares problem. When the number of inliers exceeds three, it becomes feasible to calculate a 3D linear velocity. However, due to the relative inaccuracy of height measurements, the proposed algorithm is confined to linear velocities along the x and y axes.

III-B mmWave Heatmap Preprocessing

Several methods exist for estimating rotation from the 2D intensity map, including the ICP, a classical method, and the power-azimuth method, frequently employed in mechanically spinning radars. However, given the mmWave radar’s limited detection range and irregular distribution across azimuth and elevation, the classical ICP method is suitable for use with mmWave radar. Since the heatmap from the cascade radar constitutes an unfiltered fixed point cloud involving over one million points, the initial feature extraction step necessitates simplicity. First, CFAR (CFAR) is a common preprocessing approach in radar [9], that dynamically adjusts thresholds based on local noise levels. Second, Top-k points with the highest intensity values as features, which serves as a straightforward method for feature extraction. Lastly, with the heatmap being segmented by azimuth, it permits conceptualization as a 2D LiDAR. Each ray is represented by its point of highest intensity, which are referred to as Ray-max. To the best of our knowledge, there has yet to be a successful application of feature point registration from the heatmap in single-chip radar for yaw estimation. Therefore, we compared these preprocessing techniques to solve the 2D registration challenge.

The timestamps of single-chip radar and cascade radar are different, we interpolated the linear velocity between the timestamps of single-chip radar and cascade radar data to ensure synchronization in motion estimation. When the heatmap is derived from single-chip radar data, the frame rate is same with the 4D point cloud as $dt_{s}=10$Hz. Conversely, when employing data from the cascade radar system, the frame rate is reduced to $dt_{c}=5$Hz. Under these conditions, we linearly interpolated the linear velocities which achieved between two successive cascade radar frames. This interpolated velocity is then utilized to correct translational errors in the heatmap.

$$\hat{\bm{v}}_{c}=\frac{\hat{\bm{v}}_{s_{t}}-\hat{\bm{v}}_{s_{t-1}}}{dt_{s}}\left(\frac{t_{c_{t}}+t_{c_{t-1}}}{2}-t_{s_{t-1}}\right)+\hat{\bm{v}}_{s_{t-1}}$$ (2)

$$\bm{p}_{t-1}^{rec}=\bm{p}_{t-1}-\hat{\bm{v}}_{c}dt_{c}$$ (3)

The subscript $s$ denotes single-chip radar and $c$ represents cascade radar. Consequently, we obtain rectified feature points $\bm{p}_{t-1}^{rec}$.

Despite only the rotational difference remaining between $\bm{p}_{t-1}^{rec}$ and $\bm{p}_{t}$, establishing an initial guess devoid of the IMU data and achieving a converged solution presents a significant challenge. Therefore, to attain a converged solution within a limited number of iteration, we set an error function as a weighted sum of the errors in range and azimuth:

$$e_{ij}=\alpha(r_{i}-r_{j})^{2}+\beta(\theta_{i}-\theta_{j})^{2}$$ (4)

To ensure accurate source-target correspondence, we establish two threshold parameters, $\epsilon_{max}$ and $\epsilon_{min}$, which determine the criteria for categorizing source points based on their proximity to target points. Source points are categorized as remove or neglect if they are respectively too far or too close to any target point; those remaining are considered potential matches with the closest target point, facilitating ICP computations. remove points are discarded for the iteration, while neglect points are retained for subsequent iterations. Additionally, points from the rectified previous iteration, may move beyond the radar’s observation range, designating points outside the ROI (ROI) as neglect points.

$$\bm{p}_{src}=\begin{cases}\textit{remove}&\text{if }e_{min}>\epsilon_{max}\\ \textit{neglect}&\text{if }e_{min}<\epsilon_{min}\text{ or out of \acs{ROI}}\\ \textit{match}&\text{else}\end{cases}$$ (5)

This sampling method can lead to the optimal solution faster, even avoid local minima because the neglect points can be used as matched points in the next iteration step.

Figure 3: Illustration of our key point matching step. From the intensity heatmap, preprocess the Top k points(top left) and rectify the previous frame points by estimated linear velocities(top right). After sampling the points, find the matched pair to perform weighted ICP(bottom left). Aligned points at 1st iteration in current source to previous target(bottom right). Black point on the bottom is the origin which represents the robot position.

Figure 4: Cumulative squared error of the estimated yaw in (a)EC Hallways 0 sequence and (b)Aspen 5 sequence respect to the preprocessing methods. Through the estimated 2D ego-motion, the trajectories of each sequences (c) and (d) with Top k preprocessing method. Preprocessing with one-way weighted ICP represents using the previous frame as the source and the current frame as the target.

III-C Weighted Iterative Closest Point

Leveraging the intensity information inherent to each sampled points, we can implement a weighted ICP algorithm. Since the intensity values within a heatmap can fluctuate even in a successive data, directly incorporating these values into the error function proves challenging. However, their utility as weights is beneficial. The correspondence between source and target is defined by error function, and the translation has been solved by estimated linear velocities. We applied weighted ICP, as described in [10], which utilizes normalized intensity information as weights solely for rotational registration. While denoting $\bm{x}_{i}$ and $\bm{y}_{i}$ as the source and target correspondences, it derived as

$$R^{*} =\underset{R}{\arg\min}\sum_{i}w_{i}\|\bm{y}_{i}-R\bm{x}_{i}\|^{2}$$ (6)

$$=\underset{R}{\arg\max}\sum_{i}w_{i}\bm{y}_{i}^{T}R\bm{x}_{i}=\underset{R}{\arg\max}\,\text{tr}(R^{T}H)$$

$$H =\sum_{i}w_{i}\bm{x}_{i}\bm{y}_{i}^{T}=UDV^{T},\ \ R^{*}=UV^{T}\in SO(2)$$

which the optimal rotation matrix obtained by singular value decomposition of matrix $H$. The Rotation matrix $R$ only contains yaw rotation and the weight is a normalized intensity in our case.

The process of fitting heatmap feature points to others is shown in Fig. 3. It is observed that errors in translation are effectively mitigated through the implementation of a 2D linear velocity estimation module. Additionally, the registration of sampled points demonstrates satisfactory performance, even within a single iteration. Despite the ability of the one-directional ICP algorithm to achieve convergence, it is important to note that this does not assure the optimal solution. Unlike other point cloud registration problems, the segment size of the radar feature points is not maintained and mat not persist in subsequent frames. This indicates that the results may differ based on the designated source and target. Thus, to enhance the precision in estimating yaw rates, a weighted ICP is executed bidirectionally and provides a mean rotation.

IV experiment

IV-A Experiment Environment

To verify the reliability of our method, we used ColoRadar dataset [11] which provides the heatmap and 4D point clouds from single-chip and cascade mmWave radars. They used Texas Instruments MMWCAS-RF-EVM cascade radar, AWR1843BOOST-EVM single chip radar and DCA1000-EVM for raw data capture. As we are estimating 2D linear velocity and rotation without considering vertical, roll, or pitch movements, we selected two sequences suitable for verifying our method in the ColoRadar dataset.

Dataset	Distance	Time	Environments
EC Hallway 0	106.3m	98.8s	Hallway
Aspen 5	46.7m	65.6s	Room

TABLE I: Dataset Attributes

Sensor	Range	Max.	Max.	Framerate
	resolution	Range	Azimuth
Cascade	0.06m	7.6m	76.3$\mathrm{\SIUnitSymbolDegree}$	5Hz
Single-chip	0.125m	8.0m	78.3$\mathrm{\SIUnitSymbolDegree}$	10Hz

TABLE II: mmWave Radar Attributes

IV-B Evaluation Criteria

IV-B1 Preprocessing Evaluation

Three preprocessing methods were applied to estimate yaw rate using a two-way weighted ICP. These approaches leverage identical inputs, heatmap and estimated 2D linear velocity. Therefore, the most suitable metric for assessing the efficacy of these preprocessing techniques is the evaluation of the estimated yaw error. The entire pipeline for estimating 2D ego motion proceeds independently without prior knowledge of previous frames. Thus, the assessment of these methods is conducted through the analysis of the accumulated squared error of the estimated yaw.

IV-B2 2D Trajectory

Upon identifying the optimal preprocessing technique, the complete 2D ego-motion estimation is feasible. Vertical velocity, along with roll and pitch rotations, are not estimated within this framework; it is presupposed that these components of ego-motion are pre-determined. Thus, the 2D trajectory derived from the dataset can be visualized based on the estimated ego-motion. This trajectory is built by independently estimating ego-motion for each frame. We evaluated using relative pose error, defined as

$$E_{i,j}=(P_{ref,i}^{-1}P_{ref,j})^{-1}(P_{est,i}^{-1}P_{est,j})\in SE(2)$$ (7)

by summing up the temporal relative error. These experiments are conducted within a planar trajectory context, rendering the relative pose error applicable in the SE(2). The trajectory is plotted in alignment with the ground truth data, utilizing Umeyama alignment method.

IV-C Yaw Estimation Result

During the evaluation of CFAR, Top-k points, and Ray-max preprocessing methods, it was observed that only the CFAR method exhibited variability in the number of feature points generated. We defined k of the Top-k points to be 200, and the Ray-max method implies the features same as the amount of rays, which is 128 in cascade radar. However, the CFAR method demonstrated fluctuations in the quantity of feature points, directly contributing to computational bottlenecks. We verified the rapid increase in cumulative squared yaw error of CFAR at EC hallways 0 sequence in Fig. 4. Such an increase indicates the failure of the ICP registration to converge. On the other hand, the accumulated squared error of Top-k points and Ray-max gradually increased, without any failure in both sequences. The Top-k points approach outperformed the others, and Ray-max method has a drawback where some rays omitting useful features. The RMSE (RMSE) of the estimated yaw further corroborates these findings, aligning with the observed performance trends.

Dataset	CFAR	Ray max	Top k
Aspen 5	1.86	1.94	1.35
EC hallways 0	4.66	3.09	2.06

TABLE III: Yaw RMSE [deg]

IV-D Trajectory Result

Given the superior performance of the Top-k preprocessing method, it was integrated with our proposed algorithm mwICP, which stands for the mean of the two-way weighted ICP. Under same preprocessed features, we compared our mwICP method with sampled source $p_{t-1}^{\prime}$ to sampled target $p_{t}$ weighted ICP, and the classic weighted ICP without any sampling steps. It was observed that the traditional weighted ICP failed to converge to an optimal solution, as evidenced by the divergent trajectory in Fig. 4. After using our sampling method, we found that the mwICP is more robust for the unstable feature segments of the radar point cloud. Consequently, the trajectory of the mwICP was notably enhanced compared to the one-way weighted ICP especially in the EC Hallway sequence. Through the analysis of relative pose error, it was discerned that incorporating sampling steps after preprocessing improved performance, establishing mwICP as the most precise method across both evaluated sequences.

Dataset	wICP	Sampling + wICP	Sampling + mwICP
EC hallways 0	0.0836	0.0124	0.0086
Aspen 5	0.0505	0.0077	0.0066

TABLE IV: Relative Pose Error [m]

IV-E Challenging Scenes

From the yaw error graph in Fig. 4, we observed remaining errors in our method, particularly noticeable jumping errors in certain scenes. This suggests the presence of challenging scenarios for obtaining an optimal yaw rate from point registration. Drawing from our experience, we have encountered these challenging scenes in other sequences within the ColorRadar dataset, categorized into two types, as illustrated in Fig. 5.

The first scenario concerns unstable features resulting in varying segment sizes. Preprocessed points obtained through the Top-k method depend on the parameter k, leading to inconsistent segment sizes due to the noisy clutter present in radar data. This situation is particularly common when numerous small objects are present without a dominant obstacle. In attempts to mitigate this issue, we implemented an approach utilizing the mean of the two-way weighted ICP. However, challenges persist in sequences where no suitable segment target is identified in successive frames.

The second scenario involves the curvature distortion of close points after rectification. This phenomenon is frequently observed in narrow hallways where preprocessed points densely populate the close area. After rectifying the previous frame, the coordinate system aligns with the current frame, resulting in a change in the center of rotation. As feature points typically manifest as a circular sector within a certain range, distortion during rectification worsens when feature points are in close proximity. Additionally, as the azimuth resolution increases towards both sides, this effect contributes to additional errors in narrow scenes.

Figure 5: Challenging scenes, (left)unstable features with different segment size or (right) curvature distortion after the rectification in narrow place. Black point represent the center of the sensor and the gray lines implies the detection range of the heatmap.

V Conclusion

In this work, we present an approach for estimating 2D ego-motion, including yaw rate, by matching feature points derived from mmWave radar heatmap data. The accuracy of point cloud registration was further enhanced by adopting the weighted ICP algorithm, particularly via a bidirectional methodology that accommodates both forward and reverse computations. The proposed method’s validity was substantiated by depicting trajectories derived from estimated ego-motion. While the current study focuses on the heatmap generated by cascade radar systems, future endeavors aim to extend the application of this methodology to heatmap data obtained from single-chip systems.