Pose estimation method and system
By combining a single-chip millimeter-wave radar and an inertial measurement unit, radar point clouds with enhanced angular accuracy are generated and static object points are filtered out, solving the error problem of self-pose estimation of mobile platforms in harsh environments and achieving high-precision self-pose estimation.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SHANGHAI JIAOTONG UNIV
- Filing Date
- 2023-11-07
- Publication Date
- 2026-07-14
AI Technical Summary
In existing technologies, six-DOF self-pose estimation methods for mobile platforms are difficult to accurately estimate under adverse weather conditions, poor ambient lighting, and insufficient texture features. Furthermore, methods based on 3D LiDAR are costly and heavy, making them unsuitable for lightweight and low-cost platforms.
A single-chip millimeter-wave radar combined with an inertial measurement unit is used to generate radar point clouds with enhanced angular accuracy through an FFT-MUSIC coupling algorithm. Static object points are selected using a multi-static object consensus algorithm, and angular velocity and acceleration are measured by inertial sensors to achieve self-pose estimation.
It accurately estimates the self-pose of mobile platforms in harsh environments, reducing errors from decimeter to centimeter level, overcoming the limitations of existing technologies, and is applicable to a variety of mobile platforms.
Smart Images
Figure CN117518156B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the fields of wireless sensing and ubiquitous computing technology, specifically to a self-pose estimation method and system based on FMCW millimeter-wave radar and inertial measurement unit, and more particularly to a self-pose estimation method and system. Background Technology
[0002] In recent years, intelligent mobile platforms have become increasingly prevalent in people's lives, permeating almost every aspect of daily life. Examples include delivery robots that provide last-mile delivery services for food, groceries, and parcels; cars that enhance driving safety and experience with advanced driver assistance systems; and drones that cruise at various altitudes for many other tasks such as power line inspection, crop monitoring, and damage identification after natural disasters.
[0003] Six-DOF self-pose estimation is a crucial technology enabling mobile platforms. It involves estimating the mobile platform's own three degrees of translation and three degrees of rotation. Delivery robots rely on self-pose estimation to navigate to designated indoor locations without GPS. Even in outdoor environments, cars must rely on self-pose estimation for localization in situations with poor GPS signals. In-vehicle AR devices (such as AR HUDs) use the vehicle's self-pose estimation results to project navigation information into the driver's field of vision. For drones, self-pose estimation results are important inputs to attitude adjustment and path planning modules.
[0004] To date, academia and industry have proposed a number of methods for estimating the self-pose of mobile platforms. Some of these methods rely on peripherals (such as Wi-Fi access points). Unfortunately, these methods only work within the confined area where the peripherals are located. In contrast, methods utilizing only sensors on the mobile platform (such as 3D LiDAR, cameras, and millimeter-wave radar) do not have this problem. However, 3D LiDAR-based methods often fail in adverse weather conditions (such as rain, snow, and fog). Furthermore, 3D LiDAR (approximately 1.1 kg, costing around $1600 each) is generally too bulky and expensive to deploy on lightweight and low-cost mobile platforms. Camera-based methods typically fail in environments with insufficient or excessive lighting, as well as in environments with sparse texture features.
[0005] Among all sensors typically deployed on mobile platforms, single-chip millimeter-wave radar (approximately 0.02 kg, costing around $200 each) offers key advantages such as light weight, low cost, and robustness to adverse weather and lighting conditions, leading to its increasing deployment on existing mobile platforms. Therefore, we utilize single-chip millimeter-wave radar as the primary sensor for self-pose estimation on mobile platforms. Summary of the Invention
[0006] To address the shortcomings of existing technologies, this invention provides a self-pose estimation method and system.
[0007] According to the present invention, a self-pose estimation method and system are provided, the scheme of which is as follows:
[0008] Firstly, a self-pose estimation method is provided, the method comprising:
[0009] Step S1: Collect intermediate frequency signals through millimeter-wave radar, and generate millimeter-wave radar point clouds with enhanced angle accuracy from the intermediate frequency signals using the FFT-MUSIC coupling algorithm;
[0010] Step S2: Select static object points from the millimeter-wave radar point cloud using a multi-static object consensus algorithm, and calculate the radar self-velocity based on the static object points;
[0011] Step S3: Measure the angular velocity of the moving platform using an inertial sensor, integrate the angular velocity over time, calculate the rotation angle of the moving platform at a certain moment, measure the original acceleration using an inertial sensor, and obtain the actual motion acceleration from it;
[0012] When static object points in the millimeter-wave radar point cloud can be filtered out using a multi-static object consensus algorithm, the displacement of the mobile platform is estimated using the radar self-velocity; otherwise, the displacement of the mobile platform is estimated using the actual motion acceleration measurements obtained from inertial sensors.
[0013] Preferably, step S1 includes:
[0014] Step S1.1: Perform range-Doppler two-dimensional fast Fourier transform on each frame of intermediate frequency signal collected by the millimeter-wave radar, and generate a range-Doppler matrix for each virtual antenna of the millimeter wave.
[0015] Step S1.2: Based on the constant false alarm rate algorithm, select the positions of each peak in the range-Doppler matrix. The j-th position corresponds to point P in the millimeter-wave radar point cloud. j ;
[0016] Step S1.3: For each location j where a peak exists, extract the phase of the element at that location in the range-Doppler matrix of each virtual antenna of the millimeter-wave radar. The set of these phases is denoted as Φ. j ;
[0017] Step S1.4: For each point P in the millimeter-wave radar point cloud j Based on the relative positions of the virtual antennas of the millimeter-wave radar, using Φ j The elements in the vector construct the horizontal angle vector and the pitch angle vector;
[0018] In a millimeter-wave radar virtual antenna array, let d be the mode of the horizontal spacing between a pair of virtual antennas at the same height, and let φ be the phase difference between the virtual antennas with a horizontal spacing of d.
[0019] Step S1.5: Input the horizontal and vertical angle vectors into the multi-signal classification algorithm, and the outputs are point P respectively. j The horizontal and vertical angles.
[0020] Preferably, step S2 includes:
[0021] Step S2.1: For each frame of point cloud from the radar, use a density-based clustering algorithm to cluster the point cloud, automatically dividing it into multiple point cloud clusters, so that each point cloud cluster corresponds to an object in the environment.
[0022] Step S2.2: Iterate multiple times as follows: Randomly select one point from each point cloud cluster to form a pre-selected point set, randomly select three points from the pre-selected point set as pseudo-static points, and calculate the pseudo-velocity of the millimeter-wave radar according to formula 1):
[0023]
[0024] Where, θ x，i θ y，i θ z，i Let be the angles between the i-th pseudo-static object point and the x, y, and z axes of the radar coordinate system, respectively, where i = 1, 2, and 3; v x v y v z These represent the pseudo-velocities of the radar along the x, y, and z axes in its own coordinate system; record the number of points in the pre-selected point set that satisfy formula 2):
[0025] -v i =cosθ x，i v x +cosθ y，i v y +cosθ z，i v z 2)
[0026] Step S2.3: The v obtained by satisfying the maximum number of iterations in formula 2). x v y v z Let denot be the radar's true velocity. The point that satisfies Formula 2) in this round is a static object point.
[0027] Preferably, step S3 includes:
[0028] Step S3.1: By integrating the angular velocity measured by the inertial sensor over time, calculate the rotation angle Δθ of the moving platform from time t to time t+Δt.t：t+Δt ;
[0029] Step S3.2: When the radar self-velocity can be obtained in step S2, the radar self-velocity is integrated over time to obtain the translation ΔT of the moving platform from time t to time t+Δt. t：t+Δt ;
[0030] Step S3.3: In a strong motion environment where a moving object approaches the radar and occupies the center or most of the radar's field of view, the radar's self-velocity cannot be obtained in step S2. In this case, ΔT is estimated using the measurement results from the inertial sensor. t：t+Δt .
[0031] Preferably, step S3.3 specifically includes:
[0032] Obtain the true motion acceleration from the raw acceleration measured by the inertial sensor;
[0033] Let t1 and t1+HΔt be two moments before time t when the radar self-velocity can be obtained, and their corresponding radar self-velocities are respectively and This step first uses the relative position and orientation between millimeter-wave radar and inertial sensors to determine the relative position and orientation of the sensors. and The transformation yields the self-velocity of the inertial sensor. and The inertial sensor's self-velocity, the measured initial acceleration, and the Earth's gravity satisfy Equation 3):
[0034]
[0035] in, and These are the components of Earth's gravity on the three axes of the inertial sensor at time t1+i+jΔt / K, respectively, and the actual acceleration of the inertial sensor. Δt / K is the time interval between two consecutive measurements by the inertial sensor.
[0036] The components of Earth's gravity on the three axes of the inertial sensor at time t1 Satisfies formula (4):
[0037]
[0038] Among them, R i，j The rotation matrix of the inertial sensor from t1 to t1+i+jΔt / K can be obtained by integrating the angular velocity measured by the inertial sensor over time. Combining equation 3) and formula 4), we can obtain:
[0039]
[0040] get Next, this step calculates the components of gravity along the three axes of the inertial sensor at any time between t and t+Δt according to formula 4). The actual acceleration is obtained by subtracting gravity from the acceleration measured by the inertial sensor. The actual acceleration is then integrated twice over time to obtain the translation ΔT between t and t+Δt. t：t+Δt .
[0041] Secondly, a self-pose estimation system is provided, the system comprising:
[0042] Module M1: Collects intermediate frequency signals from millimeter-wave radar and generates millimeter-wave radar point clouds with enhanced angle accuracy from the intermediate frequency signals using the FFT-MUSIC coupling algorithm;
[0043] Module M2: Uses a multi-static-object consensus algorithm to select static object points from the millimeter-wave radar point cloud and calculates the radar self-velocity based on the static object points;
[0044] Module M3: Measure the angular velocity of the mobile platform using an inertial sensor, integrate the angular velocity over time to calculate the rotation angle of the mobile platform at a certain moment, measure the raw acceleration using an inertial sensor, and obtain the actual motion acceleration from it;
[0045] When static object points in the millimeter-wave radar point cloud can be filtered out using a multi-static object consensus algorithm, the displacement of the mobile platform is estimated using the radar self-velocity; otherwise, the displacement of the mobile platform is estimated using the actual motion acceleration measurements obtained from inertial sensors.
[0046] Preferably, the module M1 includes:
[0047] Module M1.1: Performs range-Doppler two-dimensional fast Fourier transform on each frame of intermediate frequency signal collected by millimeter-wave radar, generating a range-Doppler matrix for each virtual antenna of the millimeter wave.
[0048] Module M1.2: Based on the constant false alarm rate (CFAR) algorithm, the positions of each peak in the range-Doppler matrix are selected, and the j-th position corresponds to point P in the millimeter-wave radar point cloud. j ;
[0049] Module M1.3: For each location j where a peak exists, extract the phase of the element at that location in the range-Doppler matrix of each virtual antenna of the millimeter-wave radar. The set of these phases is denoted as Φ. j ;
[0050] Module M1.4: For each point P in the millimeter-wave radar point cloud j Based on the relative positions of the virtual antennas of the millimeter-wave radar, using Φ j The elements in the vector construct the horizontal angle vector and the pitch angle vector;
[0051] In a millimeter-wave radar virtual antenna array, let d be the mode of the horizontal spacing between a pair of virtual antennas at the same height, and let φ be the phase difference between the virtual antennas with a horizontal spacing of d.
[0052] Module M1.5: Input the horizontal and vertical angle vectors into the multi-signal classification algorithm, and the outputs are point P. j The horizontal and vertical angles.
[0053] Preferably, the module M2 includes:
[0054] Module M2.1: For each frame of point cloud from the radar, a density-based clustering algorithm is used to cluster the point cloud, automatically dividing the point cloud into multiple point cloud clusters, so that each point cloud cluster corresponds to an object in the environment.
[0055] Module M2.2: Iterate multiple times using the following steps: Randomly select one point from each point cloud cluster to form a pre-selected point set; randomly select three points from the pre-selected point set as pseudo-static points; and calculate the pseudo-velocity of the millimeter-wave radar according to formula 1).
[0056]
[0057] Where, θ x，i θ y，i θ z，i Let be the angles between the i-th pseudo-static object point and the x, y, and z axes of the radar coordinate system, respectively, where i = 1, 2, and 3; v x v y v z These represent the pseudo-velocities of the radar along the x, y, and z axes in its own coordinate system; record the number of points in the pre-selected point set that satisfy formula 2):
[0058] -v i =cosθ x，i v x +cosθ y，i v y +cosθ z，i v z #2)
[0059] Module M2.3: The v obtained by satisfying the maximum number of iterations in formula 2). x v y v z Let denot be the radar's true velocity. The point that satisfies Formula 2) in this round is a static object point.
[0060] Preferably, the module M3 includes:
[0061] Module M3.1: By integrating the angular velocity measured by the inertial sensor over time, the rotation angle Δθ of the moving platform from time t to time t+Δt is calculated. t：t+Δt ;
[0062] Module M3.2: When module M2 can obtain the radar self-velocity, the radar self-velocity is integrated over time to obtain the translation ΔT of the moving platform from time t to time t+Δt. t：t+Δt ;
[0063] Module M3.3: In strong motion environments where a moving object approaches the radar, occupies the center or most of the radar's field of view, module M2 cannot obtain the radar's self-velocity. In this case, ΔT is estimated using the measurement results from the inertial sensor. t：t+Δt .
[0064] Preferably, module M3.3 specifically includes:
[0065] Obtain the true motion acceleration from the raw acceleration measured by the inertial sensor;
[0066] Let t1 and t1+HΔt be two moments before time t when the radar self-velocity can be obtained, and their corresponding radar self-velocities are respectively and This step first uses the relative position and orientation between millimeter-wave radar and inertial sensors to determine the relative position and orientation of the sensors. and The transformation yields the self-velocity of the inertial sensor. and The inertial sensor's self-velocity, the measured initial acceleration, and the Earth's gravity satisfy Equation 3):
[0067]
[0068] in, and These are the components of Earth's gravity on the three axes of the inertial sensor at time t1+i+jΔt / K, respectively, and the actual acceleration of the inertial sensor. Δt / K is the time interval between two consecutive measurements by the inertial sensor.
[0069] The components of Earth's gravity on the three axes of the inertial sensor at time t1 Satisfies formula (4):
[0070]
[0071] Among them, R i，j The rotation matrix of the inertial sensor from t1 to t1+i+jΔt / K can be obtained by integrating the angular velocity measured by the inertial sensor over time. Combining equation 3) and formula 4), we can obtain:
[0072]
[0073] get Next, this step calculates the components of gravity along the three axes of the inertial sensor at any time between t and t+Δt according to formula 4). The actual acceleration is obtained by subtracting gravity from the acceleration measured by the inertial sensor. The actual acceleration is then integrated twice over time to obtain the translation ΔT between t and t+Δt. t：t+Δt .
[0074] Compared with the prior art, the present invention has the following beneficial effects:
[0075] 1. This invention is not affected by severe weather, excessively strong or weak ambient light, or insufficient point, line, or texture features in the environment;
[0076] 2. Compared with existing self-pose estimation methods based on millimeter-wave radar, this invention can reduce the per-meter endpoint error of existing methods from the decimeter level to the centimeter level even when there are moving objects in the radar field of view.
[0077] 3. The present invention has a reasonable structure and is easy to use, and can overcome the defects of the prior art.
[0078] Other beneficial effects of the present invention will be explained in detail through the introduction of specific technical features and technical solutions in specific embodiments. Those skilled in the art should be able to understand the beneficial technical effects brought about by these technical features and technical solutions through the introduction of these technical features and technical solutions. Attached Figure Description
[0079] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings:
[0080] Figure 1 This is a schematic diagram of the method flow of the present invention;
[0081] Figure 2 This is an example diagram of the system flow in an embodiment of the present invention;
[0082] Figure 3 This is a diagram of a self-pose estimation system based on FMCW millimeter-wave radar and inertial measurement unit on a mobile robot platform in an embodiment of the present invention.
[0083] Figure 4 This is a diagram of a self-pose estimation system based on FMCW millimeter-wave radar and inertial measurement unit on an automotive platform in an embodiment of the present invention.
[0084] Figure 5 This is a diagram of a self-pose estimation system based on FMCW millimeter-wave radar and inertial measurement unit on an unmanned aerial vehicle platform in an embodiment of the present invention.
[0085] Figure 6 This is a diagram showing the experimental results of the present invention on an automotive platform. Detailed Implementation
[0086] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.
[0087] This invention provides a self-pose estimation method that can accurately estimate the self-pose of a moving platform even in challenging environments where moving objects exist within the field of view of millimeter-wave radar. This overcomes the main limitations of existing technologies and has broad application prospects. (Refer to...) Figure 1 As shown, the method specifically includes the following:
[0088] Step S1: Use the FFT-MUSIC coupling algorithm to generate millimeter-wave radar point clouds with enhanced angle accuracy from the intermediate frequency signal of millimeter-wave radar.
[0089] Step S1 specifically includes:
[0090] Step S1.1: Perform range-Doppler two-dimensional fast Fourier transform (FFT) on each frame of intermediate frequency signal collected by the millimeter-wave radar to generate a range-Doppler matrix for each virtual antenna of the millimeter wave.
[0091] Step S1.2: Based on the constant false alarm rate algorithm, select the positions of each peak in the range-Doppler matrix. The j-th position corresponds to point P in the millimeter-wave radar point cloud. j .
[0092] Step S1.3: For each location j where a peak exists, extract the phase of the element at that location in the range-Doppler matrix of each virtual antenna of the millimeter-wave radar. The set of these phases is denoted as Φ. j .
[0093] Step S1.4: For each point P in the millimeter-wave radar point cloud j Based on the relative positions of the virtual antennas of the millimeter-wave radar, using Φ j The elements in the array are used to construct the horizontal and elevation angle vectors. Let 'd' be the mode of the horizontal distance between a pair of virtual antennas at the same height in a millimeter-wave radar virtual antenna array, and 'φ' be the phase difference between virtual antennas with a horizontal distance of 'd'. The principle for constructing the horizontal angle vector is that its elements are Φ. jThe linear combination of the elements in the horizontal angle vector results in a phase difference of φ between adjacent elements. The construction method of the elevation angle vector is similar to that of the horizontal angle vector, except that the phase difference between adjacent elements in the elevation angle vector comes from the phase difference of virtual antennas with the same horizontal position but different heights.
[0094] Step S1.5: Input the horizontal and vertical angle vectors into the Multi-Signal Classification (MUSIC) algorithm, and the outputs are point P respectively. j The horizontal and vertical angles.
[0095] Step S2: Select static object points from the millimeter-wave radar point cloud using a multi-static object consensus algorithm, and calculate the radar self-velocity based on the static object points.
[0096] This step S2 specifically includes:
[0097] Step S2.1: For each frame of point cloud from the radar, use a density-based clustering algorithm (DBSCAN) to cluster the point cloud, automatically dividing it into multiple point cloud clusters, so that each point cloud cluster corresponds to an object in the environment.
[0098] Step S2.2: Iterate multiple times as follows: Randomly select one point from each point cloud cluster to form a pre-selected point set, randomly select three points from the pre-selected point set as pseudo-static points, and calculate the pseudo-velocity of the millimeter-wave radar according to formula (1):
[0099]
[0100] Where, θ x，i θ y，i θ z，i Let v be the angle between the i-th pseudo-static object point and the x, y, and z axes of the radar coordinate system, respectively. x v y v z These represent the pseudo-velocities of the radar along the x, y, and z axes in its own coordinate system. The number of points in the pre-selected point set that satisfy formula (2) is also recorded.
[0101] -v i =cosθ x，i v x +cosθ y，i v y +cosθ z，i v z (2)
[0102] Step S2.3: The v obtained from the iterations that satisfy formula (2) the most. x v y v zLet be the true self-velocity of the radar. The point that satisfies formula (2) in this round is the static object point.
[0103] Step S3: Estimate the rotation of the mobile platform using the angular velocity measurement value of the inertial measurement unit (IMU), and calibrate the acceleration measurement value of the IMU using the radar autovelocity through a synchronous calibration fusion algorithm. When the multi-static object consensus algorithm can filter out static object points in the radar point cloud, the displacement of the mobile platform is estimated using the radar autovelocity; otherwise, the displacement of the mobile platform is estimated using the acceleration measurement value of the IMU.
[0104] Step S3 specifically includes:
[0105] Step S3.1: By integrating the angular velocity measured by the inertial sensor over time, calculate the rotation angle Δθ of the moving platform from time t to time t+Δt. t：t+Δt ;
[0106] Step S3.2: When the radar self-velocity can be obtained in step S2, integrate the radar self-velocity over time to obtain the translation ΔT of the moving platform from time t to time t+Δt. t：t+Δt ;
[0107] Step S3.3: In a strong motion environment where a moving object approaches the radar and occupies the center or most of the radar's field of view, the radar's self-velocity cannot be obtained in step S2. In this case, ΔT is estimated using the measurement results from the inertial sensor. t：t+Δt Therefore, the actual acceleration must first be obtained from the raw acceleration measured by the inertial sensor. Let t1 and t1+HΔt be two moments before time t when the radar self-velocity can be obtained, and their corresponding radar self-velocities are respectively... and This step first uses the relative position and orientation between millimeter-wave radar and inertial sensors to determine the relative position and orientation of the sensors. and The transformation yields the self-velocity of the inertial sensor. and The inertial sensor's self-velocity, the measured initial acceleration, and the Earth's gravity satisfy formula (3):
[0108]
[0109] in, and Δt / K represents the components of Earth's gravity along the three axes of the inertial sensor at time t1+i+jΔt / K, and the actual acceleration of the inertial sensor, respectively. Δt / K is the time interval between two consecutive measurements by the inertial sensor. The components of Earth's gravity on the three axes of the inertial sensor at time t1 Satisfies formula (4):
[0110]
[0111] Where R i，j The rotation matrix of the inertial sensor from t1 to t1+i+jΔt / K can be obtained by integrating the angular velocity measured by the inertial sensor over time. Combining equation (3) and formula (4), we can obtain...
[0112]
[0113] get Then, this step calculates the components of gravity along the three axes of the inertial sensor at any time between t and t+Δt according to formula (4), subtracts gravity from the acceleration measured by the inertial sensor to obtain the actual motion acceleration, and integrates the actual motion acceleration twice over time to obtain the translation ΔT between t and t+Δt. t：t+Δt .
[0114] This invention also provides a self-pose estimation system, which can be implemented by executing the steps of the self-pose estimation method. That is, those skilled in the art can understand the self-pose estimation method as a preferred embodiment of the self-pose estimation system. (Refer to...) Figure 2 As shown, the system specifically includes:
[0115] Module M1: Uses the FFT-MUSIC coupling algorithm to generate millimeter-wave radar point clouds with enhanced angular accuracy from the intermediate frequency signal of millimeter-wave radar.
[0116] Module M1 specifically includes:
[0117] Module M1.1: Performs range-Doppler two-dimensional fast Fourier transform (FFT) on each frame of intermediate frequency signal collected by millimeter-wave radar, generating a range-Doppler matrix for each virtual antenna of the millimeter wave.
[0118] Module M1.2: Based on the constant false alarm rate (CFAR) algorithm, the positions of each peak in the range-Doppler matrix are selected, and the j-th position corresponds to point P in the millimeter-wave radar point cloud. j .
[0119] Module M1.3: For each location j where a peak exists, extract the phase of the element at that location in the range-Doppler matrix of each virtual antenna of the millimeter-wave radar. The set of these phases is denoted as Φ. j .
[0120] Module M1.4: For each point P in the millimeter-wave radar point cloud j Based on the relative positions of the virtual antennas of the millimeter-wave radar, using Φ jThe elements in the array are used to construct the horizontal and elevation angle vectors. Let 'd' be the mode of the horizontal distance between a pair of virtual antennas at the same height in a millimeter-wave radar virtual antenna array, and 'φ' be the phase difference between virtual antennas with a horizontal distance of 'd'. The principle for constructing the horizontal angle vector is that its elements are Φ. j The linear combination of the elements in the horizontal angle vector results in a phase difference of φ between adjacent elements. The construction method of the elevation angle vector is similar to that of the horizontal angle vector, except that the phase difference between adjacent elements in the elevation angle vector comes from the phase difference of virtual antennas with the same horizontal position but different heights.
[0121] Module M1.5: Input the horizontal and vertical angle vectors into the Multi-Signal Classification (MUSIC) algorithm, and the outputs are point P. j The horizontal and vertical angles.
[0122] Module M2: Uses a multi-static-object consensus algorithm to select static object points from the millimeter-wave radar point cloud and calculates the radar self-velocity based on the static object points.
[0123] This module M2 specifically includes;
[0124] Module M2.1: For each frame of point cloud from the radar, a density-based clustering algorithm (DBSCAN) is used to cluster the point cloud, automatically dividing it into multiple point cloud clusters, so that each point cloud cluster corresponds to an object in the environment.
[0125] Module M2.2: Iterate multiple times as follows: Randomly select one point from each point cloud cluster to form a pre-selected point set, randomly select three points from the pre-selected point set as pseudo-static points, and calculate the pseudo-velocity of the millimeter-wave radar according to formula (1):
[0126]
[0127] Where, θ x，i θ y，i θ z，i Let v be the angle between the i-th pseudo-static object point and the x, y, and z axes of the radar coordinate system, respectively. x v y v z These represent the pseudo-velocities of the radar along the x, y, and z axes in its own coordinate system. The number of points in the pre-selected point set that satisfy formula (2) is also recorded.
[0128] -v i =cosθ x，i v x +cosθ y，i v y +cosθ z，i v z (2)
[0129] Module M2.3: The v obtained from the most iterations that satisfy formula (2) x v y v z Let be the true self-velocity of the radar. The point that satisfies formula (2) in this round is the static object point.
[0130] Module M3: Estimates the rotation of the mobile platform using the angular velocity measurements from the inertial measurement unit (IMU), and calibrates the acceleration measurements from the IMU using the radar autovelocity through a synchronous calibration fusion algorithm. When the multi-static object consensus algorithm can filter out static object points in the radar point cloud, the displacement of the mobile platform is estimated using the radar autovelocity; otherwise, the displacement of the mobile platform is estimated using the acceleration measurements from the IMU.
[0131] Module M3 specifically includes:
[0132] Module M3.1: By integrating the angular velocity measured by the inertial sensor over time, the rotation angle Δθ of the moving platform from time t to time t+Δt is calculated. t：t+Δt ;
[0133] Module M3.2: When module M2 can obtain the radar self-velocity, the radar self-velocity is integrated over time to obtain the translation ΔT of the moving platform from time t to time t+Δt. t：t+Δt ;
[0134] Module M3.3: In strong motion environments where a moving object approaches the radar, occupies the center or most of the radar's field of view, module M2 cannot obtain the radar's self-velocity. In this case, ΔT is estimated using the measurement results from the inertial sensor. t：t+Δt Therefore, the actual acceleration must first be obtained from the raw acceleration measured by the inertial sensor. Let t1 and t1+HΔt be two moments before time t when the radar self-velocity can be obtained, and their corresponding radar self-velocities are respectively... and This step first uses the relative position and orientation between millimeter-wave radar and inertial sensors to determine the relative position and orientation of the sensors. and The transformation yields the self-velocity of the inertial sensor. and The inertial sensor's self-velocity, the measured initial acceleration, and the Earth's gravity satisfy formula (3):
[0135]
[0136] in, and Δt / K represents the components of Earth's gravity along the three axes of the inertial sensor at time t1+i+jΔt / K, and the actual acceleration of the inertial sensor, respectively. Δt / K is the time interval between two consecutive measurements by the inertial sensor. The components of Earth's gravity on the three axes of the inertial sensor at time t1 Satisfies formula (4):
[0137]
[0138] Where R i，j The rotation matrix of the inertial sensor from t1 to t1+i+jΔt / K can be obtained by integrating the angular velocity measured by the inertial sensor over time. Combining equation (3) and formula (4), we can obtain...
[0139]
[0140] get Then, this step calculates the components of gravity along the three axes of the inertial sensor at any time between t and t+Δt according to formula (4), subtracts gravity from the acceleration measured by the inertial sensor to obtain the actual motion acceleration, and integrates the actual motion acceleration twice over time to obtain the translation ΔT between t and t+Δt. t：t+Δt .
[0141] This invention provides a self-pose estimation method and system that can generate angle-enhanced radar point clouds from radar received signals, extract static object points from the point cloud and estimate the radar's self-velocity, and accurately estimate the self-pose of a moving platform through complementary fusion of radar and inertial measurement unit. It effectively utilizes the respective advantages of millimeter-wave radar and inertial measurement unit, and performs complementary fusion of these two sensors. In the case of difficulties in the field of view of millimeter-wave radar with moving objects, it reduces the trajectory error per meter of existing millimeter-wave radar-based self-pose estimation methods from the decimeter level to the centimeter level.
[0142] The self-pose estimation method and system provided by this invention are applicable to various mobile platforms. Examples of how to deploy this invention on different mobile platforms are as follows. Figure 3 This paper demonstrates how to deploy the method and system provided by this invention on a mobile wheeled robot. A lidar is used to acquire the true value of self-pose estimation, and a microcontroller is used to process data from millimeter-wave radar and inertial sensors in real time. Examples of deploying the method and system provided by this invention on automobiles and drones are provided respectively. Figure 4 and Figure 5 The present invention is given. Figure 4 The tests were conducted on the vehicle mobile platform shown. Specifically, as... Figure 6As shown, the test experiment included 11 tracks, each represented by the letter ak. These tracks included common road shapes such as straight lines (tracks a, e, i), turns (tracks f, g, h, j), and curves (tracks b, c, d, k). Figure 6 It can be seen that the trajectory estimation results (dark curve) obtained by the self-pose estimation method and system provided by the present invention are very close to the true trajectory values (light color).
[0143] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.
[0144] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.
Claims
1. A self-pose estimation method, characterized in that, include: Step S1: Collect intermediate frequency signals through millimeter-wave radar, and generate millimeter-wave radar point clouds with enhanced angle accuracy from the intermediate frequency signals using the FFT-MUSIC coupling algorithm; Step S2: Select static object points from the millimeter-wave radar point cloud using a multi-static object consensus algorithm, and calculate the radar self-velocity based on the static object points; Step S3: Measure the angular velocity of the moving platform using an inertial sensor, integrate the angular velocity over time, calculate the rotation angle of the moving platform at a certain moment, measure the original acceleration using an inertial sensor, and obtain the actual motion acceleration from it; When static object points in the millimeter-wave radar point cloud can be filtered out using a multi-static object consensus algorithm, the displacement of the mobile platform is estimated using radar self-velocity. Otherwise, the displacement of the moving platform is estimated by using the actual motion acceleration measurements obtained from the inertial sensor; Step S1 includes: Step S1.1: Perform range-Doppler two-dimensional fast Fourier transform on each frame of intermediate frequency signal collected by the millimeter-wave radar, and generate a range-Doppler matrix for each virtual antenna of the millimeter wave. Step S1.2: Based on the constant false alarm rate algorithm, the positions of each peak in the distance-Doppler matrix are selected. Each location corresponds to a point in the millimeter-wave radar point cloud. ; Step S1.3: For each location where a peak exists Extract the phase of the element at that location in the range-Doppler matrix of each virtual antenna of the millimeter-wave radar. The set of these phases is denoted as... ; Step S1.4: For each point in the millimeter-wave radar point cloud Based on the relative positions of the virtual antennas of the millimeter-wave radar, using The elements in the vector construct the horizontal angle vector and the pitch angle vector; Suppose that in a millimeter-wave radar virtual antenna array, the mode of the horizontal spacing between a pair of virtual antennas at the same height is... The horizontal spacing is The phase difference of the virtual antenna is ; Step S1.5: Input the horizontal and vertical angle vectors into the multi-signal classification algorithm respectively, and the outputs are points. Horizontal and vertical angles; Step S2 includes: Step S2.1: For each frame of point cloud from the radar, use a density-based clustering algorithm to cluster the point cloud, automatically dividing it into multiple point cloud clusters, so that each point cloud cluster corresponds to an object in the environment. Step S2.2: Iterate multiple times as follows: Randomly select one point from each point cloud cluster to form a pre-selected point set, and randomly select three points from the pre-selected point set as pseudo-static points. Calculate the pseudo-velocity of the millimeter-wave radar according to formula 1). 1） in, , , The first A pseudo-static object point and radar coordinate system , , The included angle of the axis, ; , , These are the radar's lower edges in its own coordinate system. , , The pseudo-velocity of the axis; record the number of points in the pre-selected set that satisfy formula 2): 2） Step S2.3: The result obtained by satisfying the maximum number of iterations in formula 2). , , Let denot be the radar's true self-velocity. The point that satisfies Formula 2) in this round is a static object point. Step S3 includes: Step S3.1: By integrating the angular velocity measured by the inertial sensor over time, the design parameters of the mobile platform are calculated. Time to Rotation angle at time ; Step S3.2: When the radar autovelocity can be obtained in step S2, the radar autovelocity is integrated over time to obtain the mobile platform. Time to Translation of time ; Step S3.3: In a strong motion environment where a moving object approaches the radar and occupies the center or most of the radar's field of view, the radar's self-velocity cannot be obtained in step S2. In this case, the radar's self-velocity is estimated using the measurement results from the inertial sensor. .
2. A self-pose estimation system, characterized in that, include: Module M1: Collects intermediate frequency signals from millimeter-wave radar and generates millimeter-wave radar point clouds with enhanced angle accuracy from the intermediate frequency signals using the FFT-MUSIC coupling algorithm; Module M2: Uses a multi-static-object consensus algorithm to select static object points from the millimeter-wave radar point cloud and calculates the radar self-velocity based on the static object points; Module M3: Measure the angular velocity of the mobile platform using an inertial sensor, integrate the angular velocity over time to calculate the rotation angle of the mobile platform at a certain moment, measure the raw acceleration using an inertial sensor, and obtain the actual motion acceleration from it; When static object points in the millimeter-wave radar point cloud can be filtered out using a multi-static object consensus algorithm, the displacement of the mobile platform is estimated using radar self-velocity. Otherwise, the displacement of the moving platform is estimated by using the actual motion acceleration measurements obtained from the inertial sensor; The module M1 includes: Module M1.1: Performs range-Doppler two-dimensional fast Fourier transform on each frame of intermediate frequency signal collected by millimeter-wave radar, generating a range-Doppler matrix for each virtual antenna of the millimeter wave. Module M1.2: Based on the constant false alarm rate algorithm, the positions of each peak in the distance-Doppler matrix are selected. Each location corresponds to a point in the millimeter-wave radar point cloud. ; Module M1.3: For each location where a peak exists Extract the phase of the element at that location in the range-Doppler matrix of each virtual antenna of the millimeter-wave radar. The set of these phases is denoted as... ; Module M1.4: For each point in the millimeter-wave radar point cloud Based on the relative positions of the virtual antennas of the millimeter-wave radar, using The elements in the vector construct the horizontal angle vector and the pitch angle vector; Suppose that in a millimeter-wave radar virtual antenna array, the mode of the horizontal spacing between a pair of virtual antennas at the same height is... The horizontal spacing is The phase difference of the virtual antenna is ; Module M1.5: Input the horizontal and vertical angle vectors into the multi-signal classification algorithm, and the outputs are points respectively. Horizontal and vertical angles; The module M2 includes: Module M2.1: For each frame of point cloud from the radar, a density-based clustering algorithm is used to cluster the point cloud, automatically dividing the point cloud into multiple point cloud clusters, so that each point cloud cluster corresponds to an object in the environment. Module M2.2: Iterate multiple times using the following steps: Randomly select one point from each point cloud cluster to form a pre-selected point set; randomly select three points from the pre-selected point set as pseudo-static points; calculate the pseudo-velocity of the millimeter-wave radar according to formula 1). 1） in, , , The first A pseudo-static object point and radar coordinate system , , The included angle of the axis, ; , , These are the radar's lower edges in its own coordinate system. , , The pseudo-velocity of the axis; record the number of points in the pre-selected set that satisfy formula 2): 2） Module M2.3: The result obtained by satisfying the maximum number of iterations in formula 2). , , Let denot be the radar's true self-velocity. The point that satisfies Formula 2) in this round is a static object point. The module M3 includes: Module M3.1: Calculates the design parameters of the mobile platform by integrating the angular velocity measured by the inertial sensor over time. Time to Rotation angle at time ; Module M3.2: When module M2 can obtain the radar autovelocity, the mobile platform is obtained by integrating the radar autovelocity over time. Time to Translation of time ; Module M3.3: In strong motion environments where a moving object approaches the radar, occupies the center or most of the radar's field of view, module M2 cannot obtain the radar's self-velocity. In this case, the radar's self-velocity is estimated using the measurement results from the inertial sensor. .