A continuous-time state estimation method for asynchronous events in space robots

Abstract

The present invention relates to a method for estimating the continuous-time state of an asynchronous event for a space robot. The asynchronous event measurement data collected by an event camera is used to achieve data association through an asynchronous feature detection and tracking front end to generate asynchronous feature trajectories, and multiple asynchronous feature trajectories are optimized and estimated using a continuous-time estimation method based on Gaussian process regression, thereby obtaining the real-time position and posture of the space robot. Since the event camera has the advantages of low power consumption, high dynamic range, event triggering, etc., it is very suitable for the space robot's requirements for energy consumption and working environment illumination. Therefore, the present invention uses an event camera to estimate the motion state of the space robot. The invented method can be used for real-time motion state estimation of the space robot in a three-dimensional space operation process with a high dynamic range, and has great application value.

Description

Technical Field

[0001] The present invention belongs to the field of space robot state estimation, and relates to an asynchronous event continuous-time state estimation method for space robots, specifically to a regression continuous-time state estimation theory and an interpolation method based on Gaussian processes. The invented method also includes an asynchronous data association method for event cameras. Background Art

[0002] In the field of space robots, the two-dimensional image information captured by the camera is usually used to restore the three-dimensional structure of the environment and estimate the motion state of the camera, thereby realizing the autonomous positioning and navigation of the space robot in three-dimensional space. However, due to the poor illumination conditions of the space environment, the dynamic range of traditional cameras is small and often difficult to meet the requirements. In addition, the amount of image data generated by traditional cameras is large, and the processing calculation is large, which makes it difficult to meet the strict requirements of space robots on energy consumption. In recent years, event cameras have been widely studied to replace traditional cameras to complete similar tasks. Event cameras are a kind of biologically inspired neuromorphic visual sensor with many advantages over traditional cameras, such as low latency, event triggering, low data volume, high dynamic range, and no motion blur. However, due to the significant difference in the underlying measurement principles of event cameras and traditional cameras, the state estimation method based on event cameras still has the following problems: on the one hand, the state estimation theory of event cameras is far from mature, and on the other hand, directly applying the state estimation method of traditional cameras to event cameras cannot fully explore the excellent characteristics of event cameras. Therefore, the development of state estimation theory based on event cameras is of great significance for developing the excellent characteristics of event cameras and improving the autonomous positioning and navigation effect of space robots.

[0003] The article "Event-based Vision: A Survey" briefly describes the principles of event cameras and their advantages over traditional cameras, and lists various applications and research on event cameras. The article lists various data association methods based on event cameras, as well as methods for event cameras to estimate 3D pose. However, there is no continuous time estimation method that directly uses the Gaussian process regression method, nor is there an asynchronous vision front end based on event trajectory tracking.

[0004] The paper "Asynchronous Optimisation for Event-based Visual Odometry" introduces an asynchronous backend optimization method for event-based visual odometry. It mentions for the first time the use of Gaussian process regression method to estimate the state of event cameras. However, the paper only focuses on the backend optimization method, and does not give a specific solution for the visual frontend. In addition, the proposed method is not real-time.

[0005] In the paper “HASTE: multi-Hypothesis Asynchronous Speeded-up Tracking of Events”, a multi-hypothesis asynchronous event stream tracking method is proposed, but it requires the location of feature points to be given in advance, and it is not possible to automatically maintain the data association process. Therefore, it is only a feature tracking method, not a complete visual front end.

[0006] The existing feature tracking methods and backend optimization methods are not perfect. The existing solutions either fail to realize the asynchronous event feature trajectory tracking of the front end, or rely on the assistance of traditional cameras and traditional discrete time backend estimation methods, which cannot accurately characterize the time when the measurement occurs, and cannot fully demonstrate the superior characteristics of event cameras compared to traditional cameras, resulting in defects such as large amount of calculation, non-real-time estimation, and inaccurate estimation. In short, there is no asynchronous event continuous time state estimation solution for space robots. Summary of the invention

[0007] Technical issues to be solved

[0008] In order to avoid the shortcomings of the prior art, the present invention proposes an asynchronous event continuous-time state estimation method for space robots, which solves the defect of insufficient research on asynchrony tracking and estimation of event camera measurement in the prior art.

[0009] Technical Solution

[0010] A method for estimating the continuous-time state of an asynchronous event for a space robot is characterized by the following steps:

[0011] Step 1: Set the sampling time interval Δt of the state trajectory, set the sliding window size n, and the state trajectory sampling point in the sliding window is x i ={x i-n+1 ,x i-n+2 ,...,x i};

[0012] Step 2: Start the asynchronous feature trajectory tracking front end. Whenever the event camera sensor outputs an event stream, the event stream is input to the front end to track the feature trajectory.

[0013] Step 3: When the tracking time of the front-end meets the threshold set by the sliding window, the system is initialized to obtain the 3D landmark points, which are reprojected to the image plane, and feature tracks whose reprojection errors are greater than the pixel threshold are eliminated;

[0014] Step 4: According to the pose estimation value of the sampling point, the state quantity at the measurement point of the feature trajectory is obtained by using the Gaussian process interpolation method; then the feature trajectory in the sliding window is triangulated, and the successfully triangulated feature trajectory is marked as completed triangulation, and the three-dimensional landmark point is obtained, and the feature trajectory whose reprojection error after triangulation is greater than the threshold is eliminated;

[0015] Step 5: Continue to obtain event stream measurements and use the front end to track and maintain feature trajectories until the time difference between the current measurement time and the latest sampling point of the sliding window is greater than or equal to the sampling time interval Δt of the state trajectory;

[0016] Step 6: Marginalize the oldest state trajectory sampling point in the sliding window, and then use the Gaussian process prior model to generate the latest state trajectory sampling point for the existing state trajectory sampling points in the sliding window and add it to the sliding window;

[0017] Step 7: Query the tracked and triangulated feature trajectory and its corresponding 3D landmark points in the current sliding window to construct the reprojection error, and add it to the Gaussian process regression continuous time backend optimizer together with the latest state trajectory sampling points generated by the motion prior in step 6, and iterate and optimize to obtain the pose estimation value of the latest state trajectory sampling points;

[0018] Step 8: Use the latest pose estimation value in step 7 to triangulate the feature tracks that have not yet been triangulated in the sliding window, mark the successfully triangulated feature tracks as having been triangulated, and obtain new 3D landmark points;

[0019] Step 9: Calculate the reprojection error of each feature trajectory, delete the feature trajectories whose reprojection error is greater than the threshold, and repeat step 5.

[0020] The threshold value of the window setting in step 3 is Δt×n.

[0021] The initialization process of step 3 is:

[0022] Step 31: Obtain the estimated value of the position and posture of each state trajectory sampling point in the sliding window;

[0023] Step 32: Perform Gaussian process interpolation calculation on the pose estimation value of the state trajectory sampling point to obtain the state quantity at the characteristic trajectory measurement point in the sliding window;

[0024] Step 33: Then, according to the state quantity at the characteristic trajectory measurement point, the characteristic trajectory in the sliding window is triangulated, and the successfully triangulated characteristic trajectory is marked as having completed triangulation, and a three-dimensional landmark point is obtained.

[0025] In step 3, the pixel value greater than the pixel threshold is 1 to 2 pixels.

[0026] A space robot, characterized in that it comprises a processor and a memory, wherein the processor is used to implement the steps of the state estimation method when executing a computer program stored in the memory.

[0027] A readable storage medium, characterized in that a computer program is stored on the readable storage medium, and the computer program implements the steps of the state estimation method when executed by a processor.

[0028] Beneficial Effects

[0029] The present invention proposes an asynchronous event continuous-time state estimation method for a space robot. The asynchronous event measurement data collected by an event camera is associated with an asynchronous feature detection and tracking front end to generate asynchronous feature trajectories. Multiple asynchronous feature trajectories are optimized and estimated using a continuous-time estimation method based on Gaussian process regression, thereby obtaining the real-time position and posture of the space robot.

[0030] Since event cameras have the advantages of low power consumption, high dynamic range, event triggering, etc., they are very suitable for the space robot's requirements on energy consumption and working environment illumination. Therefore, the present invention adopts event cameras to estimate the motion state of the space robot.

[0031] Since the measurement of the event camera is triggered by asynchronous events, a large number of measurements will be generated randomly in the time domain. Considering the limited computing resources of the space robot, the Gaussian process prior interpolation method in SE(3) local linear tangent space is used to reduce the dimension of the optimization variables and improve the computational efficiency.

[0032] The asynchronous feature tracking front-end is designed to better preserve the inherent asynchronous characteristics of the event camera and more accurately characterize the moment when the measurement is generated, which can theoretically obtain more accurate estimation results. The asynchronous feature tracking front-end discards many measurements that are irrelevant to the estimation problem, greatly reducing the amount of calculation.

[0033] Since each feature trajectory is tracked separately in the present invention, the front-end feature tracking has better parallelism, and the parallel processing unit can be used to further improve the computing efficiency.

[0034] The present invention proposes a method for estimating the continuous-time state of asynchronous events for space robots, which can track the asynchronous event measurements of the event camera as feature trajectories, and use Gaussian process regression to perform iterative optimization estimation to obtain accurate robot posture. This method can more accurately characterize the time information of the actual measurement, and fully considers the inherent asynchronous characteristics of the event camera. The invented method can be used for real-time motion state estimation of space robots in three-dimensional space operations with a high dynamic range, and has great application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 :Feature trajectory tracking effect diagram

[0036] Figure 2 : Schematic diagram of asynchronous continuous-time sliding window DETAILED DESCRIPTION

[0037] The present invention will now be further described with reference to the embodiments and the accompanying drawings:

[0038] In order to solve the problem of insufficient research on asynchronous tracking and estimation of event camera measurement in the prior art, the present invention proposes an asynchronous event continuous time state estimation method for space robots. The method is divided into two parts. The first part is the asynchronous event feature trajectory tracking front end. The front end asynchronously detects and tracks event corner points. Event corner points tracked multiple times constitute a feature trajectory. Each feature trajectory actually corresponds to the event stream tracking result of the same landmark point in the environment. Multiple feature trajectories can be used to estimate the robot motion state and landmark point map at the same time. The front end is also responsible for maintaining the addition and removal of feature trajectories, as well as the distribution of feature trajectories on the image plane, to maximize the retention of the feature trajectories most relevant to the current motion state estimation. The second part is the Gaussian process regression continuous-time backend optimizer, which uses iterative least squares optimization to solve the state information. In order to reduce the dimension of the optimization variable, the asynchronous measurements represented by the feature trajectory are parameterized as virtual measurements at adjacent time points using Gaussian process motion prior interpolation and added to the optimization one by one. In addition, the Gaussian process regression model reduces the jitter of the state trajectory to a certain extent, and the state estimate value at any time can be queried, so it belongs to continuous-time estimation.

[0039] Step 1: Set the sampling time interval (10-20 Hz) of the state trajectory to Set the sliding window size to n (the window contains 10 to 20 states), then the state trajectory sampling points in the sliding window can be expressed as χ i ={x i-n+1 ,x i-n+2 ,...,x i},in, T j Yes j The pose sampling at the moment is represented by the transformation matrix:

[0040]

[0041] Among them, T is a 4×4 transformation matrix, C is a 3×3 rotation matrix, and r is a 3×1 translation vector. is a 6-dimensional velocity vector.

[0042] Step 2 starts the asynchronous feature trajectory tracking front end. The front end is designed to be event-triggered, that is, when an event arrives, it determines whether the currently arriving event is a corner event. This requires comprehensive consideration of the time distribution of the most recent event arrivals in the field. When it is determined to be a corner time, a corner feature is initialized from the pixel coordinates of the current event, and a feature trajectory with this corner feature as the starting point is initialized, and the subsequent events in the neighborhood are used to update this feature trajectory to achieve event-based corner feature tracking. In order to facilitate asynchronous tracking and maintenance of feature trajectories, the present invention assumes that the pixel coordinates of the starting points of the most recently maintained feature trajectories are different, and the distance is greater than m pixel units, and the coordinates of the starting points of the feature trajectories are recorded in a matrix S with a size equal to the camera resolution. height×weight In , when the coordinates of the starting point are (u, v), the vth row and the uth column of the matrix S are marked as 1, and the rest are 0, and each newly initialized feature trajectory is assigned a key value of key = v*weight+u. According to the assumption, no new feature trajectory can be detected in the area of ​​size 2m+1 centered at the (u, v) position of S. This ensures that the feature trajectory is dispersed in the image plane, avoids repeated feature trajectories from being tracked simultaneously, ensures the uniqueness of the key value, and is conducive to the estimation problem. At the same time, the present invention also proposes using a matrix S of the same size as S cur To record the latest tracked position of the feature trajectory, where the latest tracking position is recorded as (key+1) to determine which feature trajectory should be tracked by each subsequently input asynchronous event, and to facilitate the query of the key value of the feature trajectory based on the pixel coordinates. The tracked feature trajectory is recorded in a map data structure. Each element in the map is a two-tuple, namely <key, fts_traj>, where key is the key value initialized previously, and fts_key is a vector that sequentially stores the time and pixel coordinates of the corner feature being tracked. When a feature trajectory jitters, its survival time exceeds the maximum allowed time, or it has not been updated for a long time, it is removed. When a new event arrives, query the matrices S and S cur The value at the corresponding position neighborhood, if it is zero, determines whether it is a new corner feature. If so, tries to create a new feature track. Otherwise, inputs the event into the tracker of the queried feature track to update the feature track.

[0043] Step 3: When the timestamp t≥t of the latest event output by the event camera i +Δt, the system is initialized. Since initialization is not the focus of the present invention, it is assumed that after initialization, the pose matrices and velocity vectors of all state estimation sampling points in the sliding window are accurately initialized, that is, after initialization, χ i The value of each element of the collection

[0044] Step 31: Obtain the estimated value of the position and posture of each state trajectory sampling point in the sliding window;

[0045] Step 32: Perform Gaussian process interpolation calculation on the pose estimation value of the state trajectory sampling point to obtain the state quantity at the characteristic trajectory measurement point in the sliding window;

[0046] Step 33: Then, according to the state quantity at the characteristic trajectory measurement point, the characteristic trajectory in the sliding window is triangulated, and the successfully triangulated characteristic trajectory is marked as having completed triangulation, and a three-dimensional landmark point is obtained.

[0047] Step 4 assumes that at the state trajectory sampling point t j and t j+1 An asynchronous measurement time stamp between is τ, that is, t j <τ<t j+1 The system state quantity is defined as in, It is the velocity vector of the body coordinate system, including rotation and translation velocity.

[0048]

[0049] Assume that the acceleration vector The Gaussian process prior followed is:

[0050]

[0051] In the formula, we define the motion prior as a constant velocity model, which is affected by a white process noise. However, since the above formula is nonlinear, in order to linearize it, j The linear tangent space near the manifold space is linearized. That is:

[0052] T(t)=T j exp(ξ j (t)^) (4)

[0053] In fact, can be used as an approximation of the body velocity, that is, So we can assume the approximate acceleration Following a Gaussian process prior:

[0054]

[0055] Therefore, the state quantity of the local linear space system in vector form can be defined as:

[0056]

[0057] The above definition is mainly to ensure the Markov property of the state quantity so as to simplify the covariance matrix. That is to say, the current state quantity is only related to the state quantity at the previous moment and the state quantity at the next moment. Formula (6) is derived with respect to time, and according to the definition, we have:

[0058]

[0059] Whenever the local linear space state quantity γ is obtained j (t), it can be restored to the nonlinear system state x(t) using the following formula:

[0060]

[0061] According to formula (4), when we know the estimated value of the system state quantity, we have The interpolation methods based on Gaussian process regression are:

[0062]

[0063] According to equations (6) and (7), Φ(t,t j ) and Q t The expression is as follows:

[0064]

[0065] When getting When the interpolation result is , according to the definition (6), we can get At the same time, according to formula (4), we have This means that the robot posture at all measurement moments can be obtained, and four feature points with adjacent parallax greater than 2 pixels on the feature trajectory are selected, and triangulated using the SVD decomposition method to obtain the 3D coordinates of the landmark points corresponding to this feature trajectory. The above triangulation operation is performed on each feature trajectory with a length greater than 5. After triangulation, the landmark points are reprojected on the entire feature trajectory using the camera observation model, and the reprojection error is calculated. When the average reprojection error of a trajectory is greater than 1 pixel, the feature trajectory is deleted, otherwise the 3D landmark points are added to the map, and the above operation is repeated until all feature trajectories in the sliding window have been triangulated once.

[0066] Step 5 executes the asynchronous feature trajectory tracking frontend described in step 2 to continue tracking and creating feature trajectories based on the input event stream until the timestamp t of the current input time satisfies t ≥ t i +Δt, where t i and Δt are defined as in step 1.

[0067] Step 6 marginalizes the oldest state trajectory sampling point in the sliding window, that is, i-n+1Discard and add x i+1 , and get a new χ i+1 ={x i-n+2 ,x i-n+3 ,...,x i+1}, where T i+1 The initial value of Obtained by the following formula:

[0068]

[0069] Among them, Φ(t i+1 ,t i ) is shown in formula (10).

[0070] Step 7 is similar to step 4. Get each 3D landmark point in the current map. Assume that the 3D coordinate variable of the kth landmark point is l k , and get l k The local linear space system state quantity expression corresponding to each tracked feature point of the corresponding feature trajectory is shown in formula (9). Assume that a certain measurement time is τ, and t j <τ<t j+1 , that is, it is located within two adjacent state sampling time periods, then the expression of the nonlinear system state quantity x(τ)=F(x j ,x j+1 ). Assume that the measurement model is h τ (x(τ),l k ), which is the projection model of the 3D landmark point. Assume that the image coordinates of the feature point measured at time τ are z τ , then the reprojection error model is:

[0071] z τ -h τ (x(τ),l k ) (12)

[0072] Assume that the covariance matrix of the measurement function is Σ τ , generally a constant symmetric matrix is ​​directly given according to the measurement sensor type, and the reprojection error energy function can be defined as:

[0073]

[0074] in It can be seen from the above formula that although our measurement occurs at a random time τ, and the number of variables of τ in the sliding window is much larger than the number of variables of the state trajectory sampling points and the landmark points, after the Gaussian process prior interpolation, the number of our optimization variables is equal to the sum of the number of state trajectory sampling points and the number of landmark points in the sliding window, which greatly simplifies the dimension of the optimization problem and improves the computational efficiency. In addition, the present invention also adds the Gaussian process motion prior as a prior energy function to the optimization problem:

[0075]

[0076] Among them, Q j+1 and Φ(t j+1 ,t j ) is obtained according to formula (10). In order to express the above energy function with nonlinear state quantity, we substitute into formula (8) and formula (10), and we have:

[0077]

[0078] Finally, the total energy function can be expressed as follows:

[0079]

[0080] Since all variables to be optimized are assigned initial values, any nonlinear least squares optimizer (such as the Ceres library) can be used to optimize the values ​​of the system state variable sampling points within the sliding window. and the value of the landmark

[0081] Step 8: After the previous step, a new state variable is obtained, so the feature tracks that have not been triangulated in the sliding window can be triangulated. First, the feature tracks that meet the following conditions are selected: 1) not triangulated; 2) the number of feature points with a parallax greater than 2 that have been tracked is greater than or equal to 4. Use the interpolation method and triangulation method similar to step 4 to triangulate the selected feature tracks.

[0082] Step 9 uses formula (12) to calculate the average reprojection error of all feature points in each feature track. The absolute value of each reprojection error is taken. If the average reprojection error of a feature track is greater than 1 pixel, it is removed from the map container created in step 2. At the same time, tables S and S are removed. cur The corresponding mark amount in , then execute step 5 until the user actively exits.

Claims

1. A method for estimating the continuous-time state of asynchronous events for space robots, characterized in that Here are the steps: Step 1: Set the sampling time interval Δt of the state trajectory, set the sliding window size n, and the state trajectory sampling point in the sliding window is x i ={x i-n+1 ,x i-n+2 ,...,x i }; Step 2: Start the asynchronous feature trajectory tracking front end. Whenever the event camera sensor outputs an event stream, the event stream is input to the front end to track the feature trajectory. Step 3: When the tracking time of the front-end meets the threshold set by the sliding window, the system is initialized to obtain the 3D landmark points, which are reprojected to the image plane, and feature tracks whose reprojection errors are greater than the pixel threshold are eliminated; Step 4: According to the pose estimation value of the sampling point, the state quantity at the measurement point of the feature trajectory is obtained by using the Gaussian process interpolation method; then the feature trajectory in the sliding window is triangulated, and the successfully triangulated feature trajectory is marked as completed triangulation, and the three-dimensional landmark point is obtained, and the feature trajectory whose reprojection error after triangulation is greater than the threshold is eliminated; Step 5: Continue to obtain event stream measurements and use the front end to track and maintain feature trajectories until the time difference between the current measurement time and the latest sampling point of the sliding window is greater than or equal to the sampling time interval Δt of the state trajectory; Step 6: Marginalize the oldest state trajectory sampling point in the sliding window, and then use the Gaussian process prior model to generate the latest state trajectory sampling point for the existing state trajectory sampling points in the sliding window and add it to the sliding window; Step 7: Query the tracked and triangulated feature trajectory and its corresponding 3D landmark points in the current sliding window to construct the reprojection error, and add it to the Gaussian process regression continuous time backend optimizer together with the latest state trajectory sampling points generated by the motion prior in step 6, and iterate and optimize to obtain the pose estimation value of the latest state trajectory sampling points; Step 8: Use the latest pose estimation value in step 7 to triangulate the feature tracks that have not yet been triangulated in the sliding window, mark the successfully triangulated feature tracks as having been triangulated, and obtain new 3D landmark points; Step 9: Calculate the reprojection error of each feature trajectory, delete the feature trajectories whose reprojection error is greater than the threshold, and repeat step 5.

2. The method for estimating the asynchronous event continuous-time state of a space robot according to claim 1, characterized in that: The threshold value of the window setting in step 3 is Δt×n.

3. The method for estimating the asynchronous event continuous-time state of a space robot according to claim 1, characterized in that: The initialization process of step 3 is: Step 31: Obtain the estimated value of the position and posture of each state trajectory sampling point in the sliding window; Step 32: Perform Gaussian process interpolation calculation on the pose estimation value of the state trajectory sampling point to obtain the state quantity at the characteristic trajectory measurement point in the sliding window; Step 33: Then, according to the state quantity at the characteristic trajectory measurement point, the characteristic trajectory in the sliding window is triangulated, and the successfully triangulated characteristic trajectory is marked as having completed triangulation, and a three-dimensional landmark point is obtained.

4. The method for estimating the asynchronous event continuous-time state of a space robot according to claim 1, characterized in that: In step 3, the pixel value greater than the pixel threshold is 1 to 2 pixels.

5. A space robot, characterized in that: The method comprises a processor and a memory, wherein the processor is used to implement the steps of the state estimation method according to any one of claims 1 to 4 when executing a computer program stored in the memory.

6. A readable storage medium, characterized in that: The readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the state estimation method according to any one of claims 1 to 4 are implemented.

Images