Mobile robot six-degree-of-freedom self-motion estimation method, system, equipment and medium

Abstract

The invention relates to the technical field of robot autonomous navigation and positioning, and discloses a mobile robot six-degree-of-freedom self-motion estimation method, system and device and a medium. The method comprises the following steps: acquiring point cloud data and Doppler velocity of a target object through a millimeter wave radar so as to acquire linear velocity of the millimeter wave radar; acquiring event data of the target object through an event camera, and constructing an optical flow field according to the motion condition of the brightness edge in the image of the target object reflected by the event data; representing a kinematic equation of each pixel point in the image of the target object in a limit constraint form, and substituting each optical flow vector in the optical flow field and the speed of the event camera into the kinematic equation in the limit constraint form to obtain the angular speed of the event camera; and performing six-degree-of-freedom self-motion state estimation on the mobile robot by combining the linear velocity of the millimeter-wave radar and the angular velocity of the event camera.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous navigation and positioning, and in particular to a method, system, device and medium for estimating six-degree-of-freedom self-motion of a mobile robot. Background Art

[0002] High-frequency, accurate estimation of the platform's six degrees of freedom (DOF) self-motion (i.e., translation and rotation in three-dimensional space) is a core capability in intelligent mobile platforms, such as robotic navigation, unmanned systems, and autonomous aircraft. This capability, often referred to as "self-motion estimation" or "motion perception," is directly related to the reliability and stability of the entire system's advanced intelligent behaviors, including positioning, mapping, obstacle avoidance, and decision-making in complex environments.

[0003] Traditional six-degree-of-freedom self-motion estimation technology mainly relies on a multi-sensor fusion system consisting of an inertial measurement unit (IMU) and a camera or lidar. This method jointly infers the robot's continuous posture and position in three-dimensional space through feature matching between image frames, IMU integration and graph optimization.

[0004] However, as the system application environment becomes increasingly dynamic and unstructured, existing solutions have gradually exposed the following key issues: (1) IMU has severe long-term drift and relies on vision for constraints, making it difficult to correct once image quality degrades; (2) The camera is prone to blurring and failure under high-speed motion, low texture, and strong light changes; (3) Traditional feature matching algorithms have high hardware requirements and are difficult to deploy in real time. Summary of the Invention

[0005] The purpose of the present invention is to provide a method, system, device and medium for estimating the six-degree-of-freedom self-motion of a mobile robot, which can solve various problems existing in the existing multi-sensor fusion system composed of IMU and camera or lidar.

[0006] To solve the above technical problems, an embodiment of the present invention provides a method for estimating six-degree-of-freedom self-motion of a mobile robot, wherein the mobile robot is equipped with a millimeter-wave radar and an event camera. The method comprises the following steps: Acquire point cloud data and Doppler velocity of the target object through millimeter wave radar; wherein there is a functional relationship between the coordinates of any point in the point cloud data and the Doppler velocity of the corresponding point and the linear velocity of the millimeter wave radar; Obtain the linear velocity of the millimeter-wave radar based on point cloud data, Doppler velocity, and functional relationship; The event data of the target object is acquired through the event camera, and the optical flow field is constructed based on the movement of the brightness edge in the image of the target object reflected by the event data; The kinematic equation of each pixel in the image of the target object is expressed in the form of an extreme constraint. Each optical flow vector in the optical flow field and the velocity of the event camera are substituted into the kinematic equation in the form of an extreme constraint to obtain the angular velocity of the event camera. The linear velocity of the millimeter-wave radar and the angular velocity of the event camera are combined to estimate the six-degree-of-freedom self-motion state of the mobile robot.

[0007] Optionally, the functional relationship is: ; Where, Any point in the point cloud data i The Doppler velocity, is the corresponding point in the point cloud data i The coordinates of for The transpose of is the linear velocity of the millimeter-wave radar; The method of obtaining the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity, and the functional relationship includes: The linear velocity of the millimeter-wave radar is determined by using the least squares method, combining the coordinates of at least three points in the point cloud data with the Doppler velocity and function relationship of the corresponding three points.

[0008] Optionally, constructing an optical flow field according to the motion of brightness edges in the image of the target object reflected by the event data includes: Constructing a time plane for reflecting the brightness edge motion of the target object in the image according to the event data; Calculate the gradient on the time plane to obtain the normal moving direction and speed of the brightness edge, so as to restore the normal optical flow of the brightness edge in the image; The optical flow field is obtained by combining the normal optical flows of two or more brightness edges with different directions using the least squares method.

[0009] Optionally, substituting each optical flow vector in the optical flow field and the velocity of the event camera into a kinematic equation in a limit constraint form to obtain the angular velocity of the event camera includes: According to the linear velocity of the lidar, the velocity of the event camera is obtained; The optical flow field is used as the derivative of the image pixel point in the kinematic equation in the limit constraint form. It is substituted into the kinematic equation in the limit constraint form together with the velocity of the event camera, and the angular velocity of the event camera is obtained using the least squares method.

[0010] Optionally, the combining of the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to perform six-degree-of-freedom self-motion state estimation on the mobile robot includes: Obtain the translation trajectory and rotation trajectory of the mobile robot based on the linear velocity of the millimeter-wave radar and the angular velocity of the event camera at each moment; The B-spline method is used to parameterize the translation trajectory and rotation trajectory. During the parameterization process, equally spaced control points are introduced as state variables within a sliding time window of a preset length to model the translation trajectory and rotation trajectory into the translation trajectory equation and rotation trajectory equation in continuous time, respectively. The translation trajectory equation and the rotation trajectory equation are integrated respectively to obtain the position and posture of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

[0011] Optionally, the combining of the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to perform six-degree-of-freedom self-motion state estimation on the mobile robot includes: An objective function is established with the goal of minimizing the error between the translation velocity obtained by the translation trajectory equation and the Doppler velocity obtained by the millimeter-wave radar, minimizing the error between the angular velocity obtained by the rotation trajectory equation and the angular velocity obtained by inverse calculation of the optical flow field of the event camera, and minimizing the error between the state variables shared in the current sliding time window and the previous sliding time window. The position and attitude obtained by integrating the translation trajectory equation and the rotation trajectory equation are optimized to obtain the target position and target attitude of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

[0012] Optionally, before obtaining the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity, and the functional relationship, the method further includes: The point cloud data is denoised by combining spatial filtering algorithm, statistical outlier removal and RANSAC algorithm.

[0013] An embodiment of the present invention further provides a six-degree-of-freedom self-motion estimation system for a mobile robot, comprising: A millimeter-wave radar for acquiring point cloud data and Doppler velocity of a target object; wherein a functional relationship exists between the coordinates of any point in the point cloud data and the Doppler velocity and linear velocity of the millimeter-wave radar at the corresponding point; Event camera, used to obtain event data of target objects; The state estimation module is used to obtain the linear velocity of the millimeter-wave radar based on point cloud data, Doppler velocity, and functional relationships; obtain event data of the target object through the event camera, and construct an optical flow field based on the movement of brightness edges in the target object's image reflected by the event data; express the kinematic equation of each pixel point in the target object's image in the form of limit constraints, substitute each optical flow vector in the optical flow field and the velocity of the event camera into the kinematic equation in the limit constraint form to obtain the angular velocity of the event camera; and combine the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot.

[0014] An embodiment of the present invention also provides a computer device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned six-degree-of-freedom self-motion estimation method of the mobile robot.

[0015] An embodiment of the present invention further provides a computer-readable storage medium storing a computer program, which implements the above-mentioned six-degree-of-freedom self-motion estimation method for a mobile robot when executed by a processor.

[0016] The method for estimating six-degree-of-freedom self-motion of a mobile robot provided by the present invention has at least the following beneficial effects: The present invention adopts an IMU-free multi-sensor fusion strategy - millimeter-wave radar and event camera. The millimeter-wave radar is used to obtain the point cloud data and Doppler velocity of the target object. There is a functional relationship between the coordinates of any point in the point cloud data and the Doppler velocity of the corresponding point and the linear velocity of the millimeter-wave radar. The linear velocity of the millimeter-wave radar can be obtained according to the point cloud data, Doppler velocity and functional relationship. The event data of the target object is obtained through the event camera, and an optical flow field is constructed according to the movement of the brightness edge in the image of the target object reflected by the event data. Then, the kinematic equation of each pixel point in the image of the target object is expressed in the form of an extreme constraint. Each optical flow vector in the optical flow field and the velocity of the event camera are substituted into the kinematic equation in the form of the extreme constraint to obtain the angular velocity of the event camera. The millimeter-wave radar and the event camera are installed on a mobile robot. Combined with the linear velocity of the millimeter-wave radar and the angular velocity of the event camera, the six-degree-of-freedom self-motion state of the mobile robot can be estimated.

[0017] This solution uses observation data from millimeter-wave radar and event cameras to estimate the six-degree-of-freedom self-motion state. Compared with traditional optical cameras, event cameras are less susceptible to high-speed motion, low texture, and strong lighting changes, and have extremely low latency. By leveraging the Doppler observation capabilities of event cameras and millimeter-wave radar, high-frequency perception of the linear and angular velocity of mobile robots can be achieved, completely getting rid of dependence on IMUs. The six-degree-of-freedom self-motion state can be estimated only through external observation data. The entire process does not require traditional feature matching or point cloud registration, which reduces computational complexity, has low hardware requirements, and can be deployed in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings: Figure 1 A flow chart of a method for estimating six-degree-of-freedom self-motion of a mobile robot provided by the present invention Figure 1 ; Figure 2 A schematic diagram of trajectory optimization based on B-spline provided by the present invention; Figure 3 A flow chart of a method for estimating six-degree-of-freedom self-motion of a mobile robot provided by the present invention Figure 2 ; Figure 4 This is a schematic diagram of the hardware composition architecture of a six-degree-of-freedom self-motion estimation method for a mobile robot provided by the present invention. DETAILED DESCRIPTION

[0019] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions of the present invention will be clearly and completely described below in conjunction with specific embodiments of the present invention and corresponding drawings. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0020] The current mainstream pose estimation algorithms are divided into three categories: IMU-based method: Motion is calculated by integrating the inertial measurement unit. The inertial measurement unit can measure the six-axis acceleration of an object. Therefore, if velocity estimation is needed, the output value can be integrated to obtain it. However, the result has cumulative drift, and the error is significant during long-term operation.

[0021] SLAM based on a single sensor: Visual SLAM: It relies on a frame-based camera to extract feature points and perform motion estimation based on the feature points. Its disadvantage is that motion blur is prone to occur during high-speed motion or drastic changes in lighting.

[0022] Radar SLAM: Estimates pose through point cloud registration, but has high computational complexity and limited ability to observe lateral and angular velocities.

[0023] Traditional fusion methods, such as the River algorithm, fuse IMU and radar data to improve accuracy through continuous-time optimization. However, they still rely on the IMU and have low efficiency in processing asynchronous data.

[0024] Disadvantages of existing technology: Dependence on IMU leads to drift: Traditional methods rely on IMU for velocity integration, and the cumulative error can reach meter levels after long-term operation.

[0025] Insufficient robustness of a single sensor: Visual cameras fail in low-texture scenes or strong light, resulting in blurred images; radar has weak observation capabilities for non-radial motion.

[0026] Asynchronous data fusion is inefficient: the time deviation between the event stream (microsecond level) and the radar data (millisecond level) leads to a decrease in fusion accuracy.

[0027] High computational complexity: Traditional point cloud registration or feature matching algorithms have high hardware requirements and are difficult to deploy in real time.

[0028] The present invention aims to provide a method for estimating six-degree-of-freedom self-motion of a mobile robot that is independent of an IMU and can efficiently process asynchronous data, thereby achieving the following goals: Improve the accuracy of pose estimation in highly dynamic environments and reduce accumulated drift; Enhance the system's robustness in low-texture, strong lighting change scenes; Simplify the calculation process and achieve lightweight real-time estimation.

[0029] The technical solutions provided by various embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0030] One embodiment of the present invention relates to a six-degree-of-freedom self-motion estimation method for a mobile robot. The mobile robot is equipped with a millimeter-wave radar (specifically a 4D millimeter-wave radar) and an event camera. The method is suitable for real-time pose estimation and trajectory optimization of intelligent devices such as drones and mobile robots in dynamic and complex environments (such as buildings, open roads, and semi-open areas). It can be applied to scenarios such as logistics distribution, emergency rescue, and urban inspections.

[0031] The specific process of the six-degree-of-freedom self-motion estimation method for a mobile robot in this embodiment can be as follows: Figure 1 As shown, including: Step 101: Acquire point cloud data and Doppler velocity of a target object through a millimeter-wave radar; wherein a functional relationship exists between the coordinates of any point in the point cloud data and the Doppler velocity of the corresponding point and the linear velocity of the millimeter-wave radar.

[0032] Step 102: Obtain the linear velocity of the millimeter wave radar based on the point cloud data, Doppler velocity, and functional relationship.

[0033] Specifically, the three-dimensional self-motion velocity (i.e., linear velocity) can be obtained based on the point cloud coordinates and Doppler velocity information in the millimeter-wave radar data, where the Doppler velocity is the projection of the platform velocity (mobile robot platform velocity) in the direction of the radar point.

[0034] Assume that a frame of point cloud data obtained at a certain moment is C, where a certain point i The coordinates in the radar coordinate system are expressed as , and the corresponding Doppler velocity measurement is defined as: ; This formula can be seen as a functional relationship between the coordinates of any point in the point cloud data and the Doppler velocity of the corresponding point and the linear velocity of the millimeter wave radar.

[0035] Where, Any point in the point cloud data i The Doppler velocity, is the corresponding point in the point cloud data i The coordinates of for The transpose of is the linear velocity of the millimeter-wave radar.

[0036] Then, the least squares method is used to combine the coordinates of at least three points in the point cloud data with the Doppler velocity and function relationship of the corresponding three points to determine the linear velocity of the millimeter wave radar.

[0037] When given n Linear velocity of self-motion when there are ≥3 effective points It can be estimated using the least squares method, and its mathematical expression is: Finally, the linear velocity of the radar in its own coordinate system is obtained as follows: ; Where, , Represents the Doppler velocity of all points in a frame.

[0038] In one example, after obtaining the point cloud data of the target object, denoising techniques are used, such as spatial filtering, statistical outlier removal, and RANSAC algorithm, to remove dynamic target interference and improve linear velocity robustness.

[0039] Step 103 : acquiring event data of the target object through an event camera, and constructing an optical flow field based on the motion of brightness edges in the image of the target object reflected by the event data.

[0040] Specifically, based on the event data, a time plane is constructed to reflect the movement of the brightness edge in the image of the target object; the gradient of the time plane is calculated to obtain the normal movement direction and speed of the brightness edge, so as to restore the normal optical flow of the brightness edge in the image; the least squares method is used to combine the normal optical flows of two or more brightness edges with different directions to obtain the optical flow field.

[0041] Step 104 , express the kinematic equation of each pixel in the image of the target object in the form of an extreme constraint, substitute each optical flow vector in the optical flow field and the velocity of the event camera into the kinematic equation in the form of the extreme constraint, and obtain the angular velocity of the event camera.

[0042] Specifically, the velocity of the event camera is obtained based on the linear velocity of the lidar; the optical flow field is used as the derivative of the image pixel points in the kinematic equation in the limit constraint form, and is substituted into the kinematic equation in the limit constraint form together with the velocity of the event camera, and the angular velocity of the event camera is obtained using the least squares method.

[0043] It can be seen that this embodiment uses the event data of the event camera to derive the angular velocity of the event camera itself. Specifically, the normal optical flow is first obtained using the original event data, and then the required angular velocity is obtained based on the extreme constraint of continuous time.

[0044] In a specific implementation, the complete process of estimating the angular velocity of an event camera using its event data is as follows: (1) Constructing the time plane: Event points reflect the motion of brightness edges in an image, and therefore can be used to construct an optical flow field. First, using event construction, a "time plane" T(x,y) is constructed within a selected time window. This plane records the timestamp of the most recent event at each pixel location. This plane reflects the trajectory of the edge in the image. A time plane (time surface) records the timestamp T(x,y) of the most recent event at each pixel location (x,y) within a certain time period, forming a two-dimensional image where each pixel value represents time information. This image reflects the spatiotemporal characteristics of the event distribution and can be used to infer the trajectory of object edges. By fitting the gradient changes in a local region within this time plane, the normal velocity of the event edge, i.e., the normal optical flow, can be recovered. This is an important intermediate quantity for subsequent angular velocity estimation.

[0045] Assume u=(x,y) is the pixel position, and an edge moves at a normal velocity Movement, then the time plane T(u) satisfy: ; The value of this function is a specific pixel u The timestamp of the latest event, where t is the current time, Indicates a point u (i.e., the pixel position on the image plane) is the signed distance to the edge, and n is the unit normal vector of the edge, which points in the direction of increasing time.

[0046] (2) Calculate the time plane gradient: In the local area near the edge, the gradient of T(x,y) is calculated to obtain , obtain the normal movement direction and speed information of the edge in the area, and then restore the normal optical flow (that is, the component of the edge movement speed projected in the direction of its normal vector).

[0047] (3) Estimation of complete optical flow (i.e. optical flow field): Based on the definition of normal optical flow, we can get: ; This represents the normal optical flow of an edge. Assuming that the optical flow is approximately constant in a short time within the local pixel area, the complete local optical flow vector is solved using two or more edge normal optical flows with different directions. If there are more than two edges in a given area, the local optical flow can be obtained using the least squares method according to the following equation: ; In the formula, the obtained is the local optical flow vector, arrive yes T The gradient representation of .

[0048] (4) Construct polar constraint equation: The angular velocity is solved using the least squares method based on the polar constraint equation.

[0049] The kinematic equation of a marker point in a given camera frame is converted into an epipolar constraint form: ; Wherein, the linear velocity vector and angular velocity vector of the event camera in its own coordinate system are The following are defined as and , represents the derivative of a point on the image, Represents the antisymmetric matrix form of a point on the image.

[0050] Under the assumption of constant illumination, the derivative of the image point It can be approximated as the optical flow provided by the event camera u(t) (That is, the optical flow vector obtained in the previous part ). The velocity in the event camera's own coordinate system can be converted from the linear velocity provided by the radar: ; Where, is the rotation matrix from the radar coordinate system to the camera coordinate system; is the rotation matrix from the body coordinate system to the camera coordinate system; is the displacement from the radar to the camera; when If it is small enough, the speed can be approximated as , which can be replaced by the linear velocity of the closest radar egomotion estimate in discrete time.

[0051] Will u(t) Substituting the velocity into the polar constraint equation we can get: , this equation can be solved by the least squares method to obtain the angular velocity: ; Where, , , A and B are matrices composed of n a and b, when there is satisfies the epipolar constraint, we can establish indivual equation.

[0052] The advantage of this method is that it can directly extract rotation information (i.e., angular velocity) from the event stream without feature matching.

[0053] In one example, the angular velocity can also be obtained by introducing a deep learning-based end-to-end optical flow estimation network, such as Ev-FlowNet or E-RAFT, constructing tensor inputs such as event volumes and time surfaces, and training a convolutional neural network to directly regress dense or sparse optical flow vector fields. This eliminates the intermediate steps of manually extracting normal information and fitting the time plane, and directly regresses the local optical flow from the event data. At the same time, the angular velocity estimate can also be solved using an optimizer such as Gaussian Newton or Bayesian filter. This approach can improve estimation accuracy in scenes with low event density and high noise.

[0054] Through the above method, the instantaneous linear velocity and angular velocity in the own coordinate system can be obtained.

[0055] Step 105 , combining the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot.

[0056] Specifically, the translation trajectory and rotation trajectory of the mobile robot are obtained according to the linear velocity of the millimeter-wave radar and the angular velocity of the event camera at each moment; the B-spline method is used to parameterize the translation trajectory and rotation trajectory, and in the process of parameterizing the translation trajectory and rotation trajectory, equally spaced control points are introduced as state variables within a sliding time window of a preset length to model the translation trajectory and rotation trajectory into translation trajectory equations and rotation trajectory equations in continuous time, respectively; the translation trajectory equation and rotation trajectory equation are integrated respectively to obtain the position and posture of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

[0057] See Figure 2 The sliding window width is T, and many control points are distributed at equal intervals in the sliding window. According to the control points, the discrete data can be constructed into the trajectory equation under continuous time, which facilitates subsequent operations such as integration.

[0058] Among them, the translation trajectory equation and rotation trajectory equation are: ; ; In the formula, considering that within a period of time, the translation trajectory equation The order of , and by the point control. is a constant that depends on the order , ; Considering that over a period of time, the rotation speed function The order of , and by the point control, .

[0059] The position and posture of the mobile robot are: ; ; Where, yes The translation matrix at time, is the translation matrix at the initial moment, yes The rotation matrix at time t, is the rotation matrix at the initial moment, represents the exponential mapping function, To accumulate the integral of the rotation part of the B-spline.

[0060] In one example, the goal of sliding window optimization is to simultaneously minimize two error terms: the predicted translational velocity, which should be consistent with the radar Doppler measurement; and the predicted angular velocity, which should be consistent with the optical flow calculated by the event camera. The approach is to construct a joint objective function that includes the Doppler velocity observation error, the event optical flow observation error, and the prior constraint error, and then minimize this function to achieve state estimation optimization.

[0061] Specifically, an objective function is established with the goals of minimizing the error between the translational velocity obtained by the translational trajectory equation and the Doppler velocity obtained by the millimeter-wave radar, minimizing the error between the angular velocity obtained by the rotational trajectory equation and the angular velocity obtained by inverting the optical flow field of the event camera, and minimizing the error between the state variables shared in the current sliding time window and the previous sliding time window; the position and attitude obtained by integrating the translational trajectory equation and the rotational trajectory equation are optimized to obtain the target position and target attitude of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

[0062] In one example, the sliding window can be replaced with a fixed-lag smoother or recursive estimation (such as the IEKF). For computationally constrained systems, simplified structures such as sliding mean filtering or motion model extrapolation can be employed. This approach reduces the real-time computational burden of back-end optimization and is suitable for edge processing devices or resource-constrained robotic platforms.

[0063] In some embodiments, the six-degree-of-freedom self-motion estimation method of the mobile robot of the present invention is achieved by Figure 3 This embodiment is implemented using the process shown in the figure. This embodiment consists of two parts: the front-end and the back-end. The front-end is composed of two methods: linear velocity estimation based on radar and angular velocity estimation based on an event camera. The velocity results generated by the front-end are then optimized in the back-end using a continuous event B-spline trajectory interpolation method. The two generated velocities are modeled as continuous-time trajectory representations, and then a sliding window optimization method is used to minimize the error. Figure 4 The hardware components of this embodiment are shown: event camera, 4D millimeter wave radar, and embedded computer, on the basis of which the above-mentioned front-end and back-end parts are realized.

[0064] The proposed method for estimating six degrees of freedom (DOF) egomotion in mobile robots aims to address the vulnerability of traditional visual-inertial methods to poorly textured, highly dynamic, and highly variable lighting environments. Specifically, it achieves high-frequency, accurate 6DOF egomotion state estimation without relying on an inertial measurement unit (IMU). Specifically, it combines Doppler velocity observations from a 4D millimeter-wave radar with high-temporal-resolution event data from an event camera to estimate the linear and angular velocities of the robot or unmanned platform in continuous time. Furthermore, a continuous trajectory representation is constructed using a cumulative B-spline model, optimized within a sliding time window, and outputs a continuous position and pose result.

[0065] It has the following advantages: IMU-free multi-sensor fusion strategy: Directly utilizes the 4D millimeter-wave radar linear velocity and event camera angular velocity, eliminating the need for an inertial measurement unit and avoiding IMU drift issues.

[0066] Asynchronous data continuous time modeling: A B-spline-based trajectory optimization framework unifies microsecond event streams and millisecond radar data into the continuous time domain to resolve the time deviation problem.

[0067] Lightweight front-end estimation algorithm: Directly solves linear velocity and angular velocity through the least squares method, skipping traditional feature matching or point cloud registration, reducing the amount of calculation by more than 50%.

[0068] As can be seen, this invention completely eliminates reliance on the IMU and can estimate the 6-DOF motion state solely through external observation data. This simplifies the sensor system, reduces costs and debugging difficulties, and is also effective in scenarios where inertial devices are unavailable. Furthermore, by introducing a continuous-time modeling approach based on cumulative B-splines, it effectively resolves the issues of temporal asynchrony and sampling rate inconsistency between the radar and event camera, enabling alignment and unified optimization of multi-source asynchronous data. Furthermore, this method features low computational complexity, a lightweight structure, and no inter-frame feature matching requirements, making it suitable for deployment on resource-constrained embedded computing platforms. It offers excellent portability and engineering application prospects.

[0069] The steps of the various methods above are divided only for the purpose of clear description. When implemented, they can be combined into one step or some steps can be split and decomposed into multiple steps. As long as they include the same logical relationship, they are within the scope of protection of the present invention. Adding insignificant modifications or introducing insignificant designs to the algorithm or process without changing the core design of the algorithm and process are all within the scope of protection of the invention.

[0070] Another embodiment of the present invention relates to a six-degree-of-freedom self-motion estimation system for a mobile robot. The implementation details of the six-degree-of-freedom self-motion estimation system for a mobile robot of this embodiment are described in detail below. The following content is only provided for ease of understanding and is not required for the implementation of this solution. The six-degree-of-freedom self-motion estimation system for a mobile robot of this embodiment includes: A millimeter-wave radar for acquiring point cloud data and Doppler velocity of a target object; wherein a functional relationship exists between the coordinates of any point in the point cloud data and the Doppler velocity and linear velocity of the millimeter-wave radar at the corresponding point; Event camera, used to obtain event data of target objects; The state estimation module is used to obtain the linear velocity of the millimeter-wave radar based on point cloud data, Doppler velocity, and functional relationships; obtain event data of the target object through the event camera, and construct an optical flow field based on the movement of brightness edges in the target object's image reflected by the event data; express the kinematic equation of each pixel point in the target object's image in the form of limit constraints, substitute each optical flow vector in the optical flow field and the velocity of the event camera into the kinematic equation in the limit constraint form to obtain the angular velocity of the event camera; and combine the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot.

[0071] It is not difficult to find that this embodiment is a system embodiment corresponding to the above-mentioned method embodiment, and this embodiment can be implemented in conjunction with the above-mentioned method embodiment. The relevant technical details and technical effects mentioned in the above-mentioned embodiment are still valid in this embodiment, and to reduce repetition, they are not repeated here. Accordingly, the relevant technical details mentioned in this embodiment can also be applied to the above-mentioned embodiment.

[0072] It is worth noting that all modules involved in this embodiment are logical modules. In actual applications, a logical unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. In addition, to highlight the innovations of the present invention, this embodiment does not include units that are not closely related to solving the technical problems proposed by the present invention. However, this does not mean that other units do not exist in this embodiment.

[0073] Another embodiment of the present invention relates to a computer device, comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the six-degree-of-freedom self-motion estimation method for a mobile robot in the above-mentioned embodiments.

[0074] The memory and processor are connected using a bus, which can include any number of interconnected buses and bridges. The bus connects various circuits of one or more processors and memories. The bus can also connect various other circuits such as peripheral devices, voltage regulators, and power management circuits. These are all well known in the art and are therefore not described further herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be a single component or multiple components, such as multiple receivers and transmitters, providing a unit for communicating with various other devices over a transmission medium. Data processed by the processor is transmitted over a wireless medium via an antenna. Furthermore, the antenna receives data and transmits it to the processor.

[0075] The processor is responsible for managing the bus and general processing, and can also provide various functions, including timing, peripheral interfaces, voltage regulation, power management, and other control functions. Memory can be used to store data used by the processor when performing operations.

[0076] Another embodiment of the present invention relates to a computer-readable storage medium storing a computer program, which implements the above method embodiment when executed by a processor.

[0077] That is, those skilled in the art will understand that all or part of the steps in the above-described method embodiments can be implemented by instructing the relevant hardware through a program. The program is stored in a storage medium and includes a number of instructions for causing a device (such as a microcontroller or chip) or a processor to execute all or part of the steps in the method embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0078] Those skilled in the art will appreciate that the above embodiments are specific embodiments for implementing the present invention, and that in actual applications, various changes may be made thereto in form and detail without departing from the spirit and scope of the present invention.

Claims

1. A method for estimating six-degree-of-freedom self-motion of a mobile robot, characterized in that: The mobile robot is equipped with a millimeter wave radar and an event camera, and the method includes: Acquire point cloud data and Doppler velocity of the target object through millimeter wave radar; wherein there is a functional relationship between the coordinates of any point in the point cloud data and the Doppler velocity of the corresponding point and the linear velocity of the millimeter wave radar; Obtain the linear velocity of the millimeter-wave radar based on point cloud data, Doppler velocity, and functional relationship; The event data of the target object is acquired through the event camera, and the optical flow field is constructed based on the movement of the brightness edge in the image of the target object reflected by the event data; The kinematic equation of each pixel in the image of the target object is expressed in the form of an extreme constraint. Each optical flow vector in the optical flow field and the velocity of the event camera are substituted into the kinematic equation in the form of an extreme constraint to obtain the angular velocity of the event camera. The linear velocity of the millimeter-wave radar and the angular velocity of the event camera are combined to estimate the six-degree-of-freedom self-motion state of the mobile robot.

2. The method for estimating six-degree-of-freedom self-motion of a mobile robot according to claim 1, wherein: The functional relationship is: ； Where, Any point in the point cloud data i The Doppler velocity, is the corresponding point in the point cloud data i The coordinates of for The transpose of is the linear velocity of the millimeter-wave radar; The method of obtaining the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity, and the functional relationship includes: The linear velocity of the millimeter-wave radar is determined by using the least squares method, combining the coordinates of at least three points in the point cloud data with the Doppler velocity and function relationship of the corresponding three points.

3. The method for estimating six-degree-of-freedom self-motion of a mobile robot according to claim 1, wherein: The step of constructing an optical flow field based on the motion of brightness edges in the image of the target object reflected by the event data includes: Constructing a time plane for reflecting the brightness edge motion of the target object in the image according to the event data; Calculate the gradient on the time plane to obtain the normal moving direction and speed of the brightness edge, so as to restore the normal optical flow of the brightness edge in the image; The optical flow field is obtained by combining the normal optical flows of two or more brightness edges with different directions using the least squares method.

4. The method for estimating six-degree-of-freedom self-motion of a mobile robot according to claim 3, wherein: Substituting each optical flow vector in the optical flow field and the velocity of the event camera into the kinematic equation in the limit constraint form to obtain the angular velocity of the event camera includes: According to the linear velocity of the lidar, the velocity of the event camera is obtained; The optical flow field is used as the derivative of the image pixel point in the kinematic equation in the limit constraint form. It is substituted into the kinematic equation in the limit constraint form together with the velocity of the event camera, and the angular velocity of the event camera is obtained using the least squares method.

5. The method for estimating six-degree-of-freedom self-motion of a mobile robot according to claim 1, wherein: The method combines the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot, including: Obtain the translation trajectory and rotation trajectory of the mobile robot based on the linear velocity of the millimeter-wave radar and the angular velocity of the event camera at each moment; The B-spline method is used to parameterize the translation trajectory and rotation trajectory. During the parameterization process, equally spaced control points are introduced as state variables within a sliding time window of a preset length to model the translation trajectory and rotation trajectory into the translation trajectory equation and rotation trajectory equation in continuous time, respectively. The translation trajectory equation and the rotation trajectory equation are integrated respectively to obtain the position and posture of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

6. The method for estimating six-degree-of-freedom self-motion of a mobile robot according to claim 5, wherein: The method combines the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot, including: An objective function is established with the goal of minimizing the error between the translation velocity obtained by the translation trajectory equation and the Doppler velocity obtained by the millimeter-wave radar, minimizing the error between the angular velocity obtained by the rotation trajectory equation and the angular velocity obtained by inverse calculation of the optical flow field of the event camera, and minimizing the error between the state variables shared in the current sliding time window and the previous sliding time window. The position and attitude obtained by integrating the translation trajectory equation and the rotation trajectory equation are optimized to obtain the target position and target attitude of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

7. The method for estimating six-degree-of-freedom self-motion of a mobile robot according to claim 1, wherein: Before obtaining the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity, and the functional relationship, the method further includes: The point cloud data is denoised by combining spatial filtering algorithm, statistical outlier removal and RANSAC algorithm.

8. A six-degree-of-freedom self-motion estimation system for a mobile robot, characterized in that: The system comprises: A millimeter-wave radar for acquiring point cloud data and Doppler velocity of a target object; wherein a functional relationship exists between the coordinates of any point in the point cloud data and the Doppler velocity and linear velocity of the millimeter-wave radar at the corresponding point; Event camera, used to obtain event data of target objects; The state estimation module is used to obtain the linear velocity of the millimeter-wave radar based on point cloud data, Doppler velocity, and functional relationships; obtain event data of the target object through the event camera, and construct an optical flow field based on the movement of brightness edges in the target object's image reflected by the event data; express the kinematic equation of each pixel point in the target object's image in the form of limit constraints, substitute each optical flow vector in the optical flow field and the velocity of the event camera into the kinematic equation in the limit constraint form to obtain the angular velocity of the event camera; and combine the linear velocity of the millimeter-wave radar and the angular velocity of the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot.

9. A computer device, characterized in that: include: at least one processor; And, a memory communicatively connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the six-degree-of-freedom self-motion estimation method for a mobile robot as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method for estimating six-degree-of-freedom self-motion of a mobile robot according to any one of claims 1 to 7 is implemented.

Images