Mobile robot six-degree-of-freedom ego-motion estimation method, system, device, and medium

Abstract

The application 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, device and medium. The method comprises the following steps: acquiring point cloud data and Doppler velocity of a target object through a millimeter wave radar to obtain the 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 the kinematic equation of each pixel point in the image of the target object in the form of limit constraint, substituting each optical flow vector in the optical flow field and the speed of the event camera into the kinematic equation in the form of limit constraint to obtain the angular velocity of the event camera; and combining 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.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot autonomous navigation and positioning, in particular to a mobile robot six-degree-of-freedom self-motion estimation method, system, device and medium. BACKGROUND

[0002] In intelligent mobile platforms such as robot navigation, unmanned systems, and autonomous aircraft, achieving high-frequency and accurate estimation of the six-degree-of-freedom self-motion state (i.e., translation and rotation in three-dimensional space) of the platform is a core fundamental capability. This capability is often referred to as "self-motion estimation" or "motion perception", and is directly related to the reliability and stability of high-level intelligent behaviors such as positioning, mapping, obstacle avoidance, and decision-making in complex environments.

[0003] Traditional six-degree-of-freedom self-motion estimation techniques mainly rely on multi-sensor fusion systems composed of inertial measurement units (IMU) and cameras or lidar. Such methods infer the continuous pose and position of the robot in three-dimensional space through feature matching between image frames, IMU integration, and graph optimization.

[0004] However, as the dynamicity and unstructuredness of the system application environment continue to increase, existing solutions have gradually exposed the following key problems:

[0005] (1) IMU long-term drift is severe, and relies on vision for constraint, which is difficult to correct once the image quality decreases;

[0006] (2) Cameras are prone to blurring and failure under high-speed motion, low texture, and strong light changes;

[0007] (3) Traditional feature matching algorithms have high hardware requirements and are difficult to deploy in real time. SUMMARY

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

[0009] To solve the above technical problems, an embodiment of the present application provides a mobile robot six-degree-of-freedom self-motion estimation method, wherein a millimeter wave radar and an event camera are installed on the mobile robot, and the method comprises the following steps:

[0010] Obtain point cloud data and Doppler velocity of the target object through the 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;

[0011] According to the point cloud data, the Doppler velocity and the function relationship, the linear velocity of the millimeter wave radar is obtained;

[0012] The event data of the target object is obtained by the event camera, and according to the motion of the brightness edge in the image of the target object reflected by the event data, a flow field is constructed.

[0013] The kinematic equation of each pixel point in the image of the target object is expressed in the form of limit constraint, each flow vector in the flow field and the speed of the event camera are substituted into the kinematic equation in the form of limit constraint, and the angular velocity of the event camera is obtained.

[0014] The six-degree-of-freedom self-motion state estimation of the mobile robot is performed in combination with the linear velocity of the millimeter wave radar and the angular velocity of the event camera.

[0015] Optionally, the function relationship is:

[0016] ;

[0017] In the formula, is the Doppler velocity of any point i in the point cloud data, is the coordinate of the corresponding point i in the point cloud data, is the transpose of , and is the linear velocity of the millimeter wave radar.

[0018] The linear velocity of the millimeter wave radar is obtained according to the point cloud data, the Doppler velocity and the function relationship, including:

[0019] The least square method is used to determine the linear velocity of the millimeter wave radar in combination with the coordinates of at least three points in the point cloud data, the Doppler velocities of the corresponding three points and the function relationship.

[0020] Optionally, the flow field is constructed according to the motion of the brightness edge in the image of the target object reflected by the event data, including:

[0021] According to the event data, a time plane for reflecting the motion of the brightness edge in the image of the target object is constructed.

[0022] The gradient of the time plane is calculated to obtain the normal moving direction and speed of the brightness edge, so as to restore the normal flow of the brightness edge in the image.

[0023] The least square method is used to obtain the flow field in combination with the normal flows of two or more brightness edges in different directions.

[0024] Optionally, the substituting each optical flow vector in the optical flow field and the speed of the event camera into the kinematics equation in the form of limit constraint obtains the angular velocity of the event camera, and the method comprises the following steps of:

[0025] According to the linear velocity of the laser radar, the speed of the event camera is obtained.

[0026] The optical flow field is taken as the derivative of the image pixel point in the kinematics equation in the form of limit constraint, and the speed of the event camera is substituted into the kinematics equation in the form of limit constraint, and the least square method is used to obtain the angular velocity of the event camera.

[0027] Optionally, the six-degree-of-freedom self-motion state estimation of the mobile robot is performed by combining the linear velocity of the millimeter wave radar and the angular velocity of the event camera, and the method comprises the following steps of:

[0028] According to the linear velocity of the millimeter wave radar and the angular velocity of the event camera at each time, the translation trajectory and the rotation trajectory of the mobile robot are obtained.

[0029] The B-spline method is used to parameterize the translation trajectory and the rotation trajectory, and in the process of parameterizing the translation trajectory and the rotation trajectory, the control points distributed at equal intervals in the preset length of the sliding time window are introduced as state variables, so that the translation trajectory and the rotation trajectory are respectively modeled into the translation trajectory equation and the rotation trajectory equation under continuous time.

[0030] The translation trajectory equation and the rotation trajectory equation are integrated respectively to obtain the position and the attitude of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

[0031] Optionally, the six-degree-of-freedom self-motion state estimation of the mobile robot is performed by combining the linear velocity of the millimeter wave radar and the angular velocity of the event camera, and the method comprises the following steps of:

[0032] The target function is established by taking the minimum error between the translation velocity obtained through the translation trajectory equation and the Doppler velocity obtained through the millimeter wave radar, the minimum error between the angular velocity obtained through the rotation trajectory equation and the angular velocity obtained through the optical flow field of the event camera, and the minimum error between the state variables shared by the current sliding time window and the last sliding time window as the target.

[0033] The position and the attitude obtained by integrating the translation trajectory equation and the rotation trajectory equation are optimized to obtain the target position and the target attitude of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

[0034] Optionally, before the linear velocity of the millimeter wave radar is obtained according to the point cloud data, the Doppler velocity and the function relationship, the method further comprises the following steps of:

[0035] The point cloud data is denoised by combining a spatial filtering algorithm, statistical outlier rejection and a RANSAC algorithm.

[0036] Embodiments of the application also provide a mobile robot six-degree-of-freedom self-motion estimation system, comprising:

[0037] A millimeter wave radar is configured to acquire point cloud data and Doppler velocity of a target object, wherein a function 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.

[0038] An event camera is configured to acquire event data of the target object.

[0039] A state estimation module is configured to acquire the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity and the function relationship, acquire the event data of the target object through the event camera, construct an optical flow field according to the motion of the brightness edges in the image of the target object reflected by the event data, express the kinematic equation of each pixel in the image of the target object in a limit constraint form, 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 perform 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.

[0040] Embodiments of the application also provide a computer device, comprising at least one processor and a memory in communication connection with 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 perform the above-mentioned mobile robot six-degree-of-freedom self-motion estimation method.

[0041] Embodiments of the application also provide a computer readable storage medium storing a computer program, wherein the computer program is executed by a processor to implement the above-mentioned mobile robot six-degree-of-freedom self-motion estimation method.

[0042] The mobile robot six-degree-of-freedom self-motion estimation method provided by the application has at least the following beneficial effects:

[0043] The application adopts a multi-sensor fusion strategy without IMU, namely millimeter wave radar and event camera, acquires point cloud data and Doppler velocity of the target object through the millimeter wave radar, and 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, so that the linear velocity of the millimeter wave radar can be acquired according to the point cloud data, the Doppler velocity and the functional relationship, acquires event data of the target object through the event camera, and constructs an optical flow field according to the motion of the brightness edge in the image of the target object reflected by the event data, then represents the kinematic equation of each pixel point in the image of the target object in the form of limit constraint, substitutes each optical flow vector in the optical flow field and the velocity of the event camera into the kinematic equation in the form of limit constraint to obtain the angular velocity of the event camera, and the millimeter wave radar and the event camera are installed on the mobile robot, so that the linear velocity of the millimeter wave radar and the angular velocity of the event camera can be combined to estimate the six-degree-of-freedom self-motion state of the mobile robot.

[0044] The scheme selects the observation data of the millimeter wave radar and the event camera to estimate the six-degree-of-freedom self-motion state, wherein the event camera is less affected by high-speed motion, low texture and strong light changes compared with a traditional optical camera, and has extremely low delay, and the high-frequency perception of the linear velocity and the angular velocity of the mobile robot can be realized by using the Doppler observation ability of the event camera and the millimeter wave radar, the dependence on the IMU is completely eliminated, the estimation of the six-degree-of-freedom self-motion state can be completed only through external observation data, the whole process does not need traditional feature matching or point cloud registration, the calculation complexity is reduced, the hardware requirement is low, and real-time deployment can be realized. BRIEF DESCRIPTION OF DRAWINGS

[0045] The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this application, illustrate embodiments of the application and serve to explain the principles of the application, and do not limit the application in any way. In the drawings:

[0046] Figure 1 A flowchart of a mobile robot six-degree-of-freedom self-motion estimation method provided by the application Figure 1

[0047] Figure 2 A trajectory optimization schematic diagram based on B-spline provided by the application

[0048] Figure 3 A flowchart of a mobile robot six-degree-of-freedom self-motion estimation method provided by the application Figure 2

[0049] Figure 4 A hardware composition architecture schematic diagram of a mobile robot six-degree-of-freedom self-motion estimation method provided by the application DETAILED DESCRIPTION​​

[0050] In order to make the objects, technical solutions and advantages of the present application clearer, the technical solutions of the present application will be described clearly and completely below in combination with specific embodiments of the present application and corresponding drawings. Obviously, the described embodiments are only some of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0051] The current mainstream pose estimation algorithm is divided into three categories:

[0052] IMU-based method: the motion is calculated by integrating the inertial measurement unit, the inertial measurement unit can measure the 6-axis acceleration of the object, so if the speed estimation is needed, the output value can be integrated to obtain it, but the result has cumulative drift, and the error is significant in long-time operation.

[0053] SLAM based on a single sensor:

[0054] Visual SLAM: relying on frame cameras to extract feature points, and estimating motion according to feature points, the disadvantage is that motion blur is easy to occur in high-speed motion or severe light changes.

[0055] Radar SLAM: estimating pose by point cloud registration, but the calculation complexity is high, and the observation ability of lateral speed and angular speed is limited.

[0056] Traditional fusion method: such as River algorithm fuses IMU and radar data, and improves the accuracy by continuous time optimization, but still relies on IMU and has low processing efficiency for asynchronous data.

[0057] Disadvantages of the prior art:

[0058] Dependence on IMU leads to drift: the traditional method relies on IMU for speed integration, and the cumulative error can reach meters after long-time operation.

[0059] Single sensor robustness is insufficient: the vision camera will fail in low-texture scenes or strong light, resulting in image blur; the radar has weak observation ability for non-radial motion.

[0060] Low efficiency of asynchronous data fusion: the time deviation of event stream (microsecond level) and radar data (millisecond level) leads to a decrease in fusion accuracy.

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

[0062] The present application aims to provide a mobile robot six-degree-of-freedom self-motion estimation method without dependence on IMU and efficient processing of asynchronous data, and to achieve the following objectives:

[0063] improve the pose estimation accuracy in high dynamic environment and reduce the cumulative drift;

[0064] enhance the robustness of the system in low-texture and strong light change scenes;

[0065] simplify the calculation process and realize lightweight real-time estimation.

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

[0067] One embodiment of the present application relates to a mobile robot six-degree-of-freedom self-motion estimation method, a millimeter wave radar (specifically a 4D millimeter wave radar) and an event camera are installed on the mobile robot, which is suitable for real-time pose estimation and trajectory optimization of intelligent devices such as unmanned aerial vehicles and mobile robots in dynamic and complex environments (such as buildings, open roads, and semi-open areas), and can be applied to logistics distribution, emergency rescue, city inspection, and other scenes.

[0068] The specific process of the mobile robot six-degree-of-freedom self-motion estimation method of the present embodiment can be as shown in Figure 1 , which includes:

[0069] Step 101: obtaining point cloud data and Doppler velocity of a target object through a 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.

[0070] Step 102: obtaining the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity, and the functional relationship.

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

[0072] Suppose a frame of point cloud data obtained at a certain time is C, wherein the coordinates of a certain point in the radar coordinate system are represented as i , and the measured value of the corresponding Doppler velocity is defined as:

[0073] ;

[0074] This formula can be regarded 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.

[0075] In the formula, is the Doppler velocity of any point i in the point cloud data, ​coordinates of corresponding points in the point cloud data, i , , , is a linear velocity of the millimeter wave radar.

[0076] Then, the linear velocity of the millimeter wave radar is determined by using a least square method in combination with the coordinates of at least three points in the point cloud data and the Doppler velocities of the corresponding three points and the functional relationship.

[0077] When the number of valid points is given as n ≥ 3, the linear velocity of the ego motion can be estimated by using a least square method, and the mathematical expression is as follows: and the linear velocity of the radar in the ego coordinate system is finally obtained, as shown in the following formula:

[0078] ;

[0079] In the formula, v is the Doppler velocity of the radar in the ego coordinate system, and , represents the Doppler velocity of all points in a frame.

[0080] In an example, after obtaining the point cloud data of the target object, a denoising technology is used, for example, a spatial filtering, statistical outlier rejection and RANSAC algorithm are used to remove dynamic target interference, so as to improve the robustness of the linear velocity.

[0081] In step 103, event data of the target object is obtained by using an event camera, and a flow field is constructed according to the motion of the brightness edges in the image of the target object reflected by the event data.

[0082] Specifically, a time plane for reflecting the motion of the brightness edges in the image of the target object is constructed according to the event data; the normal moving direction and the speed of the brightness edges are obtained by calculating the gradient of the time plane, so as to restore the normal flow of the brightness edges in the image; and the flow field is obtained by using a least square method in combination with the normal flow of two or more brightness edges in different directions.

[0083] In step 104, the kinematic equation of each pixel point in the image of the target object is expressed in a limit constraint form, each flow vector in the flow field and the speed of the event camera are substituted into the kinematic equation in the limit constraint form, and the angular velocity of the event camera is obtained.

[0084] Specifically, the speed of the event camera is obtained according to the linear velocity of the laser radar; the flow field is taken as the derivative of the image pixel point in the kinematic equation in the limit constraint form, and the speed of the event camera is substituted into the kinematic equation in the limit constraint form, and the angular velocity of the event camera is obtained by using a least square method.

[0085] It can be seen that the 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 by using the original event data, and then the required angular velocity is obtained based on the polar constraint of continuous time.

[0086] In a specific implementation, the complete process of estimating the angular velocity of the event camera through the event data of the event camera is as follows:

[0087] (1) Construct a time plane:

[0088] The event point reflects the motion of the brightness edge in the image, so the optical flow field can be constructed based on this. First, the event is used to construct a "time plane" T(x, y) in a selected time window, that is, each pixel position records the timestamp of the latest event occurrence. The plane reflects the motion trajectory of the edge in the image. The time surface records the timestamp T(x, y) of the latest event occurrence of each pixel position (x, y) in a certain time period, forming a two-dimensional image, where each pixel value is time information. This image reflects the spatiotemporal characteristics of event distribution and can be used to infer the moving trajectory of the object edge. By fitting the gradient change of the local area in the time plane, the normal moving speed of the event edge, that is, the normal optical flow, can be recovered, which is an important intermediate quantity for subsequent angular velocity estimation.

[0089] Assume that u=(x,y) is a pixel position, and a certain edge moves with a normal speed , then the time plane T(u) satisfies: ; the value of the function is the timestamp of the latest event of a certain pixel u , in the formula, t is the current time, , which represents the signed distance from the point u (that is, the pixel position on the image plane) to the edge, and n is the unit normal vector of the edge, which points to the direction of time increment.

[0090] (2) Calculate the gradient of the time plane:

[0091] In the local area near the edge, the gradient of T(x, y) is calculated to obtain , which obtains the normal moving direction and speed information of the edge in the region, and further recovers the normal optical flow (that is, the component of the edge movement projected on the normal vector direction).

[0092] (3) Estimate the complete optical flow (that is, the optical flow field):

[0093] Based on the definition of the normal optical flow, the following can be obtained:

[0094] ;

[0095] This represents the normal optical flow of an edge. Assuming that the optical flow is approximately constant in a local pixel region in a short time, the complete local optical flow vector can be solved by using two or more edge normal optical flows with different directions. If there are more than two edges in a given region, the local optical flow can be obtained by using the least square method according to the following equation:

[0096]

[0097] In the equation, the obtained is the local optical flow vector, is the gradient of T .

[0098] (4) Construct the epipolar constraint equation:

[0099] According to the epipolar constraint equation, the angular velocity can be solved by using the least square method.

[0100] The kinematic equation of a marker point in the given camera system is converted into the epipolar constraint form:

[0101]

[0102] In the equation, the linear velocity vector and the angular velocity vector of the event camera in its own coordinate system are defined as and , respectively, and denotes the derivative of a point on the image, and denotes the skew-symmetric matrix form of the point on the image.

[0103] Under the assumption of constant illumination, the derivative of the image point u(t) can be approximated as the optical flow provided by the event camera (i.e., the optical flow vector solved in the previous part). The velocity in the coordinate system of the event camera can be converted from the linear velocity provided by the radar:

[0104]

[0105] In the equation, 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 is small enough, the velocity can be approximated as , and here the linear velocity closest to the radar self-motion estimation at the discrete time can be used instead.

[0106] will​​​u(t) Substituting the velocity into the polar constraint equation, we can obtain: The angular velocity can be solved using the least squares method:

[0107] ;

[0108] In the formula, , A and B are matrices composed of n a and b elements. When there are A system that satisfies the epipolar constraint can be established. indivual equation.

[0109] The advantage of this method is that it extracts rotational information (i.e., angular velocity) directly from the event stream without requiring feature matching.

[0110] In one example, the aforementioned angular velocity can also be obtained by introducing an end-to-end optical flow estimation network based on deep learning, such as Ev-FlowNet or E-RAFT, to construct tensors such as event volume and time surface as inputs. A convolutional neural network is then trained to directly regress the dense or sparse optical flow vector field, eliminating the intermediate steps of manually extracting normal information and fitting the time plane, and directly regressing the local optical flow from the event data. Simultaneously, the angular velocity estimation can also be solved using optimizers such as Gauss-Newton or Bayesian filters. This method can improve the estimation accuracy in low-event-density, high-noise scenarios.

[0111] Using the above method, the instantaneous linear velocity and angular velocity in its own coordinate system can be obtained.

[0112] Step 105: Combine the linear velocity from the millimeter-wave radar and the angular velocity from the event camera to estimate the six-degree-of-freedom self-motion state of the mobile robot.

[0113] Specifically, the translational and rotational trajectories of the mobile robot are obtained based on the linear velocity of the millimeter-wave radar and the angular velocity of the event camera at various times. The B-spline method is used to parameterize the translational and rotational trajectories. During the parameterization of the translational and rotational trajectories, equally spaced control points are introduced as state variables within a preset sliding time window to model the translational and rotational trajectories as translational and rotational trajectory equations in continuous time, respectively. The translational and rotational trajectory equations are integrated to obtain the position and attitude of the mobile robot, which serve as the six-degree-of-freedom self-motion state estimation results.

[0114] See also Figure 2 The sliding window has a width of T, and many control points are distributed at equal intervals within the sliding window. Based on the control points, discrete data can be constructed into a trajectory equation in continuous time, which facilitates subsequent integration and other operations.

[0115] where the translational trajectory equation and the rotational trajectory equation are respectively:

[0116] ;

[0117] ;

[0118] where the translational trajectory equation is of order and is controlled by the point . is a constant depending on the order , ; the rotational velocity function is of order and is controlled by the point , .

[0119] The position and the pose of the mobile robot are respectively:

[0120] ;

[0121] ;

[0122] where is the translation matrix at time , is the translation matrix at initial time , is the rotation matrix at time , is the rotation matrix at initial time , denotes the exponential mapping function, is the integral of the cumulative B-spline rotation part. In one example, the purpose of the sliding window optimization is to minimize two error terms simultaneously: the first is the predicted translational velocity, which should be consistent with the radar Doppler measurements; the second is the predicted angular velocity, which should be consistent with the optical flow results calculated by the event camera. The specific method is to construct a joint objective function containing the Doppler velocity observation error, the event optical flow observation error and the prior constraint error, and to realize the state estimation optimization by minimizing the function.

[0123] In one example, the purpose of the sliding window optimization is to minimize two error terms simultaneously: the first is the predicted translational velocity, which should be consistent with the radar Doppler measurements; the second is the predicted angular velocity, which should be consistent with the optical flow results calculated by the event camera. The specific method is to construct a joint objective function containing the Doppler velocity observation error, the event optical flow observation error and the prior constraint error, and to realize the state estimation optimization by minimizing the function.

[0124] ​Specifically, an objective function is established with the objectives of minimizing the error between the translational velocity obtained through the translational trajectory equation and the Doppler velocity obtained through millimeter-wave radar, minimizing the error between the angular velocity obtained through the rotational trajectory equation and the angular velocity obtained by back-calculation from the optical flow field of the event camera, and minimizing the error between the state variables shared between 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, which are used as the six-degree-of-freedom self-motion state estimation results.

[0125] In one example, the sliding window can be replaced with fixed-hysteresis smoothing or recursive estimation (such as IEKF). For computationally limited systems, simplified structures such as moving mean filtering or motion model extrapolation can be used. This approach reduces the real-time computational burden of backend optimization and is suitable for edge processing devices or resource-constrained robotic platforms.

[0126] In some embodiments, the mobile robot six-degree-of-freedom self-motion estimation method of the present invention uses... Figure 3 The process is illustrated in this embodiment, which is divided into two parts: a front-end and a back-end. The front-end consists of two methods—radar-based linear velocity estimation and event camera-based angular velocity estimation. The velocity results generated by the front-end are then optimized in the back-end. The optimization method uses continuous event B-spline trajectory interpolation to model the two velocities into 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: an event camera, a 4D millimeter-wave radar, and an embedded computer, which implement the aforementioned front-end and back-end components.

[0127] This invention presents a six-DOF self-motion estimation method for mobile robots, aiming to address the problem of traditional vision-inertial methods failing in environments with weak texture, high dynamics, and strong lighting variations. Specifically, it achieves high-frequency, accurate six-DOF self-motion state estimation without relying on an inertial measurement unit (IMU). By jointly utilizing Doppler velocity observations from a 4D millimeter-wave radar and high temporal resolution event data from an event camera, the linear and angular velocities of the robot or unmanned platform are estimated continuously. Furthermore, a continuous trajectory representation is constructed using a cumulative B-spline model, and optimization is performed within a sliding time window to output continuous pose results at all times.

[0128] It has the following advantages:

[0129] IMU-free multi-sensor fusion strategy: directly utilize the linear velocity of 4D millimeter-wave radar and the angular velocity of event camera, without the need for an inertial measurement unit, thus avoiding IMU drift problems.

[0130] Asynchronous data continuous time modeling: based on the B-spline trajectory optimization framework, the microsecond-level event stream and millisecond-level radar data are unified to the continuous time domain, and the time bias problem is solved.

[0131] Lightweight front-end estimation algorithm: the linear velocity and angular velocity are directly solved by the least square method, and the traditional feature matching or point cloud registration is skipped, and the calculation amount is reduced by more than 50%.

[0132] It can be seen that the application completely gets rid of the dependence on the IMU, and the estimation of the 6-degree-of-freedom motion state can be completed only through external observation data, the sensor system is simplified, the cost and debugging difficulty are reduced, and the application can also play a role in some scenes where inertial devices are not available. And by introducing the continuous time modeling method based on cumulative B-spline, the time asynchronous and sampling rate inconsistency problems between the radar and the event camera are effectively solved, the alignment and unified optimization of multi-source asynchronous data are realized. In addition, the method has small calculation amount, light structure, no inter-frame feature matching requirement, is suitable for deployment on a resource-limited embedded computing platform, has good portability and engineering application prospect.

[0133] The step division of the above various methods is only for the purpose of clear description, and when implemented, one step can be combined or some steps can be split into multiple steps, as long as the same logical relationship is included, and it is within the protection scope of the application; adding irrelevant modifications or introducing irrelevant designs in the algorithm or process, but not changing the core design of the algorithm and process are within the protection scope of the application.

[0134] Another embodiment of the application relates to a mobile robot six-degree-of-freedom self-motion estimation system, and the implementation details of the mobile robot six-degree-of-freedom self-motion estimation system of the embodiment will be specifically described below. The following content is only provided for the implementation details for the convenience of understanding, and is not necessary for implementing the scheme. The mobile robot six-degree-of-freedom self-motion estimation system of the embodiment comprises:

[0135] A millimeter wave radar is used to acquire point cloud data and Doppler velocity of a target object; 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;

[0136] An event camera is used to acquire event data of the target object;

[0137] The state estimation module is used for obtaining the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity and a function relationship; event data of the target object is obtained through the event camera, and a flow field is constructed according to the motion condition of the brightness edge in the image of the target object reflected by the event data; the kinematic equation of each pixel point in the image of the target object is expressed in the form of limit constraint, each flow vector in the flow field and the velocity of the event camera are substituted into the kinematic equation in the form of limit constraint, and the angular velocity of the event camera is obtained; the six-degree-of-freedom self-motion state estimation of the mobile robot is performed in combination with the linear velocity of the millimeter wave radar and the angular velocity of the event camera.

[0138] It can be found that the embodiment corresponds to the method embodiment, and the embodiment can be implemented in cooperation with the method embodiment. The related technical details and technical effects mentioned in the above embodiments are still valid in the embodiment, and to reduce repetition, they are not described here. Accordingly, the related technical details mentioned in the embodiment can also be applied in the above embodiments.

[0139] It is worth mentioning that each module involved in the embodiment is a logical module, and in actual application, a logical unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. In addition, in order to highlight the innovative part of the application, units not closely related to solving the technical problems proposed in the application are not introduced in the embodiment, but this does not mean that there are no other units in the embodiment.

[0140] Another embodiment of the application relates to a computer device, comprising: at least one processor; and a memory connected with the at least one processor in communication; 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 perform the mobile robot six-degree-of-freedom self-motion estimation method in each of the above embodiments.

[0141] The memory and the processor are connected in a bus mode, the bus can include any number of interconnected buses and bridges, and the bus connects various circuits of one or more processors and memories together. The bus can also connect various other circuits such as peripheral devices, voltage stabilizers and power management circuits together, which are well known in the art, and therefore, they will not be further described herein. The bus interface provides an interface between the bus and the transceiver. The transceiver can be one element or multiple elements such as multiple receivers and transmitters, which provide a unit for communicating with various other devices on the transmission medium. The data processed by the processor is transmitted on the wireless medium through the antenna, and further, the antenna also receives data and transmits the data to the processor.

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

[0143] Another embodiment of the present application relates to a computer readable storage medium storing a computer program. The computer program is executed by a processor to implement the method embodiments.

[0144] That is, those skilled in the art can understand that all or part of the steps of the above-mentioned method embodiments can be completed by programs instructing relevant hardware, the programs are stored in a storage medium, and the storage medium includes a plurality of instructions for causing an apparatus (which can be a single-chip microcomputer, a chip, etc.) or a processor to execute all or part of the steps of the method described in each embodiment of the present application. The foregoing storage medium includes a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various media that can store program codes.

[0145] Those skilled in the art can understand that the above-mentioned embodiments are specific embodiments for implementing the present application, and in actual applications, various changes can be made in form and details without departing from the spirit and scope of the present application.

Claims

1. A mobile robot six degree of freedom self-motion estimation method, characterized by, The mobile robot is provided with a millimeter wave radar and an event camera, and the method comprises: acquiring point cloud data and Doppler velocity of the target object by the millimeter wave radar; wherein a function 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; acquiring the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity and the function relationship; acquiring event data of the target object by the event camera, and constructing an optical flow field according to the motion of the brightness edge in the image of the target object reflected by the event data; expressing the kinematic equation of each pixel point in the image of the target object in a limit constraint form, 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; combining 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; wherein the function relationship is: ； In the formula, For any point in the point cloud data i Doppler velocity, For the corresponding points in the point cloud data i coordinates for transpose, The linear velocity of the millimeter-wave radar; the acquiring the linear velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity and the function relationship comprises: determining the linear velocity of the millimeter wave radar by combining the coordinates of at least three points in the point cloud data, the Doppler velocity of the corresponding three points and the function relationship by using a least square method; the constructing an optical flow field according to the motion of the brightness edge in the image of the target object reflected by the event data comprises: constructing a time plane for reflecting the motion of the brightness edge in the image of the target object according to the event data; obtaining the normal moving direction and velocity of the brightness edge by taking the gradient of the time plane to restore the normal optical flow of the brightness edge in the image; and obtaining the optical flow field by combining the normal optical flows of two or more brightness edges with different directions by using a least square method; the 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 comprises: obtaining the velocity of the event camera according to the linear velocity of the laser radar; substituting the optical flow field as the derivative of the image pixel point in the kinematic equation in the limit constraint form into the kinematic equation in the limit constraint form together with the velocity of the event camera; and obtaining the angular velocity of the event camera by using a least square method.

2. The mobile robot six degrees of freedom self-motion estimation method of claim 1, wherein, the combining 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 comprises: acquiring the translational trajectory and the rotational trajectory of the mobile robot according to the linear velocity of the millimeter wave radar and the angular velocity of the event camera at each time; parameterizing the translational trajectory and the rotational trajectory by using a B-spline method, and introducing equidistantly distributed control points as state variables in a preset length of a sliding time window in the process of parameterizing the translational trajectory and the rotational trajectory to model the translational trajectory and the rotational trajectory into translational trajectory equations and rotational trajectory equations respectively under continuous time; integrating the translational trajectory equations and the rotational trajectory equations respectively to obtain the position and the attitude of the mobile robot as the six-degree-of-freedom self-motion state estimation result.

3. The mobile robot six degrees of freedom self-motion estimation method of claim 2, wherein, the combining 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 comprises: A target function is established with the objective 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 backstepping the optical flow field of the event camera, and minimizing the error between the state variables shared by the current sliding time window and the last 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.

4. The mobile robot six degrees of freedom self-motion estimation method of claim 1, wherein, Before the line velocity of the millimeter wave radar is obtained 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 a spatial filtering algorithm, statistical outlier rejection, and a RANSAC algorithm.

5. A mobile robot six degree of freedom self-motion estimation system, characterized by, The system includes: a millimeter wave radar configured to obtain 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 of the corresponding point and the line velocity of the millimeter wave radar; an event camera configured to obtain event data of the target object; a state estimation module configured to obtain the line velocity of the millimeter wave radar according to the point cloud data, the Doppler velocity, and the functional relationship, obtain event data of the target object by the event camera, construct an optical flow field according to the motion of the brightness edges in the image of the target object reflected by the event data, express the kinematic equation of each pixel in the image of the target object in the form of limit 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 limit constraint to obtain the angular velocity of the event camera, and perform six-degree-of-freedom self-motion state estimation on the mobile robot in combination with the line velocity of the millimeter wave radar and the angular velocity of the event camera; wherein the functional relationship is: ； wherein is the Doppler velocity of a point in the point cloud data, i is the coordinate of a corresponding point in the point cloud data, i is the transpose of is the linear velocity of the millimeter wave radar;​​​ The state estimation module is further configured to: determine the line velocity of the millimeter wave radar by the least square method in combination with the coordinates of at least three points in the point cloud data, the Doppler velocity of the corresponding three points, and the functional relationship; construct a time plane for reflecting the motion of the brightness edges in the image of the target object according to the event data, obtain the normal moving direction and velocity of the brightness edges by taking the gradient of the time plane to restore the normal optical flow of the brightness edges in the image, and obtain the optical flow field by the least square method in combination with the normal optical flow of two or more brightness edges in different directions; obtain the velocity of the event camera according to the line velocity of the laser radar, substitute the optical flow field as the derivative of the image pixel in the kinematic equation in the form of limit constraint and the velocity of the event camera into the kinematic equation in the form of limit constraint, and obtain the angular velocity of the event camera by the least square method.

6. A computer device, comprising: includes: at least one processor; and a memory in communication with 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 perform the six-degree-of-freedom self-motion estimation method of the mobile robot according to any one of claims 1 to 4.

7. A computer readable storage medium storing a computer program, characterized in that, The computer program is executed by a processor to implement the mobile robot six-degree-of-freedom self-motion estimation method in any one of claims 1 to 4.

Images