CN111354043A - A three-dimensional attitude estimation method and device based on multi-sensor fusion - Google Patents

Abstract

The invention provides a three-dimensional attitude estimation method and device based on multi-sensor fusion. The method includes: acquiring an RGB image of a measured object, extracting FAST feature points and tracking the feature points; collecting IMU measurement values of the measured object , and integrate to obtain the position, velocity and rotation of the measured object at the current moment; obtain the depth image of the measured object, and process the depth image to obtain the iterative closest point; based on the prior information, the iterative closest point, the IMU measurement value and An objective function is constructed by visual reprojection; an iterative method is used to solve the minimum value of the objective function to obtain the optimal pose of the object. This method performs attitude estimation based on a variety of sensors, and constructs an objective function for attitude estimation. The objective function is solved by the least square method. The input value of the function is input through a sliding window, and the specific number of sliding windows is set. The solution time is reduced and the estimation accuracy of the pose is improved.

Description

A three-dimensional attitude estimation method and device based on multi-sensor fusion

technical field

The invention relates to the technical field of three-dimensional reconstruction, in particular to a three-dimensional attitude estimation method and device based on multi-sensor fusion.

Background technique

Attitude estimation problem is to determine the orientation of a three-dimensional target object. Pose estimation has applications in many fields such as robot vision, motion tracking, and single-camera calibration. The sensors used for pose estimation in different fields are different.

Model-based methods usually use the geometric relationship of objects or the feature points of objects to estimate. The basic idea is to use a certain geometric model or structure to represent the structure and shape of the object, and by extracting some object features, establish a corresponding relationship between the model and the image, and then realize the estimation of the object's spatial pose through geometric or other methods. . The model used here may be a simple geometric shape, such as a plane, a cylinder, or a certain geometric structure, or a three-dimensional model obtained by laser scanning or other methods. The model-based pose estimation method is to update the pose of the object by comparing the real image and the synthetic image, and calculating the similarity. In order to avoid the optimization search in the global state space, the current model-based methods generally degrade the optimization problem into a matching problem of multiple local features, which is very dependent on the accurate detection of local features. When the noise is too large to extract accurate local features, the robustness of the method is greatly affected.

Learning-based methods use machine learning methods to learn the correspondence between 2D observations and 3D poses from previously acquired training samples in different poses, and apply the learned decision rules or regression functions to sample, and the result is used as the pose estimation for the sample. Learning-based methods generally use global observation features, do not need to detect or identify local features of objects, and have better robustness. The disadvantage is that the accuracy and continuity of pose estimation cannot be guaranteed because the dense sampling required for continuous estimation in high-dimensional space cannot be obtained.

SUMMARY OF THE INVENTION

The present invention proposes the following technical solutions in view of the above-mentioned defects in the prior art.

A three-dimensional pose estimation method based on multi-sensor fusion, the method includes:

The RGB image acquisition step, using an image sensor to acquire the RGB image of the measured object, and extracting the FAST feature points of the RGB image and using the KLT sparse optical flow method to track the feature points;

The IMU measurement value acquisition step is to use the IMU sensor to collect the IMU measurement value of the measured object, and integrate the IMU measurement value to obtain the position, speed and rotation of the measured object at the current moment;

In the depth image acquisition step, a depth image sensor is used to acquire a depth image of the measured object, and an iterative closest point (ICP) algorithm is used to process the depth image to obtain an iterative closest point;

Objective function construction step, construct objective function based on prior information, iterative closest point, IMU measurement value and visual reprojection;

In the pose estimation step, an iterative method is used to solve the minimum value of the objective function to obtain the optimal pose of the object.

Further, the method further includes: a loop closure detection step, detecting whether a loop closure occurs in the current frame, and if so, performing a global pose graph optimization to correct the positioning trajectory of the object to eliminate accumulated errors.

Further, the objective function is:

min _(R,t) {PP+∑||D|| ² +∑||I|| ² +∑||V|| ² };

和位移参数

使四个残差项PP、∑||D||²、∑||I||²与∑||V||²的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差。A most suitable motion parameter is obtained by the least square method: rotation parameter

and displacement parameters

The value of the sum of the four residual terms PP, ∑||D|| ² , ∑||I|| ² and ∑||V|| ² is the lowest, where PP represents the prior distribution of prior information, D represents the iterative nearest point residual term, I represents the difference in the amount of change in position, velocity, rotation, gyroscope bias and acceleration bias between two frames of the object, and V represents the reprojection error.

Further, the least squares are solved based on the sliding window, the sliding window contains the input parameters required for the calculation of the least squares, and the error term of each least squares except the prior information is provided by the value in the sliding window. .

Further, the keyframes in the RGB image are determined based on the following conditions:

The difference between the average disparity of the frame and the average disparity of the previous key frame exceeds the first threshold;

Alternatively, the number of feature points tracked in this frame is less than the second threshold.

Furthermore, the geometric structure of the space is obtained from the depth image, thereby estimating the transformation matrix between the two frames, and the iterative closest point (ICP) residual term D uses the point-to-surface distance as:

D=(T·P _j -P _i )·n _i

和位移参数

是两帧之间的运动参数；Among them, P _j and P _i are the matched 3D points in the above depth image, n _i is the normal vector corresponding to the previous frame, and the rotation parameter

and displacement parameters

is the motion parameter between two frames;

The reprojection error V serves as a visual constraint and defines it on the tangent plane of the unit sphere:

是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，

和

是P对应的切平面的任意两个正交基。in,

is the predicted value of the unit vector of the three-dimensional point after the feature point is transformed by the rigid body, P is the three-dimensional point obtained by the back-projection of the feature point by the pixel coordinates through the pinhole camera model,

and

are any two orthonormal bases of the tangent plane corresponding to P.

The present invention also proposes a three-dimensional attitude estimation device based on multi-sensor fusion, the device comprising:

The RGB image acquisition unit is used to obtain the RGB image of the measured object using the image sensor, and extracts the FAST feature point of the RGB image and uses the KLT sparse optical flow method to track the feature point;

The IMU measurement value acquisition unit is used to use the IMU sensor to collect the IMU measurement value of the measured object, and integrate the IMU measurement value to obtain the position, speed and rotation of the measured object at the current moment;

a depth image acquisition unit, used for using a depth image sensor to acquire a depth image of the measured object, and using an iterative closest point (ICP) algorithm to process the depth image to obtain an iterative closest point;

An objective function construction unit for constructing an objective function based on prior information, iterative closest points, IMU measurements and visual reprojection;

The pose estimation unit is configured to use an iterative method to solve the minimum value of the objective function to obtain the optimal pose of the object.

Furthermore, a loop closure detection unit is used to detect whether a loop closure occurs in the current frame, and if so, perform global pose graph optimization to correct the positioning trajectory of the object to eliminate accumulated errors.

Further, the objective function is:

min _(R,t) {PP+∑||D|| ² +∑||I|| ² +∑||V|| ² };

和位移参数

使四个残差项PP、∑||D||²、∑||I||²与∑||V||²的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差。A most suitable motion parameter is obtained by the least square method: rotation parameter

and displacement parameters

The value of the sum of the four residual terms PP, ∑||D|| ² , ∑||I|| ² and ∑||V|| ² is the lowest, where PP represents the prior distribution of prior information, D represents the iterative nearest point residual term, I represents the difference in the amount of change in position, velocity, rotation, gyroscope bias and acceleration bias between two frames of the object, and V represents the reprojection error.

Further, the least squares are solved based on the sliding window, the sliding window contains the input parameters required for the calculation of the least squares, and the error term of each least squares except the prior information is provided by the value in the sliding window. .

Further, the keyframes in the RGB image are determined based on the following conditions:

The difference between the average disparity of the frame and the average disparity of the previous key frame exceeds the first threshold;

Alternatively, the number of feature points tracked in this frame is less than the second threshold.

Furthermore, the geometric structure of the space is obtained from the depth image, thereby estimating the transformation matrix between the two frames, and the iterative closest point (ICP) residual term D uses the point-to-surface distance as:

D=(T·P _j -P _i )·n _i

和位移参数

是两帧之间的运动参数；Among them, P _j and P _i are the matched 3D points in the above depth image, n _i is the normal vector corresponding to the previous frame, and the rotation parameter

and displacement parameters

is the motion parameter between two frames;

The reprojection error V serves as a visual constraint and defines it on the tangent plane of the unit sphere:

是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，

和

是P对应的切平面的任意两个正交基。in,

is the predicted value of the unit vector of the three-dimensional point after the feature point is transformed by the rigid body, P is the three-dimensional point obtained by the back-projection of the feature point by the pixel coordinates through the pinhole camera model,

and

are any two orthonormal bases of the tangent plane corresponding to P.

The technical effect of the present invention is: a three-dimensional attitude estimation method based on multi-sensor fusion of the present invention, the method includes: using an image sensor to obtain an RGB image of a measured object, extracting FAST feature points of the RGB image, and using KLT The sparse optical flow method is used to track the feature points; the IMU measurement value of the measured object is collected by the IMU sensor, and the IMU measurement value is integrated to obtain the position, speed and rotation of the measured object at the current moment; the depth image sensor is used to obtain The depth image of the measured object, the iterative closest point (ICP) algorithm is used to process the depth image to obtain the iterative closest point; the objective function is constructed based on the prior information, the iterative closest point, the IMU measurement value and the visual reprojection; the iterative method is used The optimal pose of the object is obtained by solving the minimum value of the objective function. This method uses various sensors to collect RGB images, IMU measurement values and depth images of moving objects for attitude estimation, and constructs an objective function for attitude estimation. The objective function is solved by the least square method. The input value of the function is obtained by The sliding window is used for input, the specific number of sliding windows is set, the solution time is reduced, the estimation accuracy of the pose is improved, and the loopback detection process is also designed, and the global pose graph optimization is performed when a loopback occurs to correct the positioning of the object trajectory to eliminate the accumulated error, and specifically design the corresponding residual function.

Description of drawings

Other features, objects and advantages of the present application will become more apparent upon reading the detailed description of non-limiting embodiments taken with reference to the following drawings.

FIG. 1 is a flowchart of a three-dimensional pose estimation method based on multi-sensor fusion according to an embodiment of the present invention.

FIG. 2 is a structural diagram of a three-dimensional attitude estimation apparatus based on multi-sensor fusion according to an embodiment of the present invention.

Detailed ways

The present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the related invention, but not to limit the invention. In addition, it should be noted that, for the convenience of description, only the parts related to the related invention are shown in the drawings.

It should be noted that the embodiments in the present application and the features of the embodiments may be combined with each other in the case of no conflict. The present application will be described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.

Fig. 1 shows a three-dimensional attitude estimation method based on multi-sensor fusion of the present invention, and the method includes:

RGB image acquisition step S101, use an image sensor to acquire the RGB image of the measured object, and extract the FAST feature point of the RGB image and use the KLT sparse optical flow method to track the feature point; the image sensor can be a camera, a video camera, etc. etc., it can collect multiple frames of images of the measured object.

Whenever a new frame of RGB image arrives, the system will automatically extract FAST feature points, and then use the KLT sparse optical flow method to track the feature points. In order to ensure the accuracy of tracking and keep the computational cost relatively small, the number of feature points to be tracked can be set between 100 and 300, and new feature points will be detected during feature point tracking to maintain the number of tracked feature points. stay within a threshold.

IMU measurement value acquisition step S102, use the IMU sensor to collect the IMU measurement value of the measured object, and integrate the IMU measurement value to obtain the position, speed and rotation of the measured object at the current moment; the IMU sensor is also called an inertial sensor, By integrating the discrete IMU measurements, the position, velocity, and rotation at the current moment can be obtained, and the motion estimation of the object currently equipped with the sensor can be given.

Step S103 of obtaining a depth image, using a depth image sensor to obtain a depth image of the measured object, and using an iterative closest point (ICP) algorithm to process the depth image to obtain an iterative closest point; the depth image sensor may be a depth camera, a depth camera , the geometric structure of the measured object space can be obtained through the depth image, so as to estimate the transformation matrix between the two frames.

In addition, the system's estimation of the real scale of the scene is also done through the depth image. The depth image can provide the real distance of the pixel relative to the camera's optical center (Pinhole), so that the pixel coordinates and spatial coordinates can be converted through the pinhole camera model, so as to restore the real scale of the scene.

Although the acquisition frequency of the IMU measurement value in S102 is higher than the acquisition frequency of the depth image in S103, in general, since the IMU data and the image data need to be aligned, the IMU measurement value accumulated within the time difference between two adjacent frames of depth images Not enough points required. The present invention accumulates the IMU measurement value in the image acquisition interval of four consecutive frames, so as to avoid the fluctuation of the integration result caused by insufficient IMU measurement data. However, the iterative closest point obtained by processing the depth images separated by four frames has a larger error. The present invention retains the iterative closest point calculation results of adjacent frames, and combines the iterative closest point calculation results of four consecutive frames as the result of the adjacent four frames. Iterate the closest point result.

The objective function construction step S104 is to construct an objective function based on the prior information, the iterative closest point, the IMU measurement value and the visual reprojection. Since each sensor has its own inapplicable environment, the environment during motion cannot be controlled by humans. In order to improve the robustness in multiple scenarios and obtain more accurate positioning, the present invention adopts multi-sensor data fusion to improve the accuracy and robustness of pose estimation.

The present invention comprehensively utilizes three kinds of sensor information such as RGB image, depth image, and IMU measurement value in a tightly-coupled manner to estimate the optimal attitude estimation between two frames, and solves the least squares problem in an iterative manner.

Specifically, the objective function is:

min _(R,t) {PP+∑||D|| ² +∑||I|| ² +∑||V|| ² };

和位移参数

使四个残差项PP、∑||D||²、∑||I||²与∑||V||²的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差，即PP、D、I、V也可以称为目标函数的四个约束，也就是四个残差项。A most suitable motion parameter is obtained by the least square method: rotation parameter

and displacement parameters

The value of the sum of the four residual terms PP, ∑||D|| ² , ∑||I|| ² and ∑||V|| ² is the lowest, where PP represents the prior distribution of prior information, D Represents the iterative nearest point residual term, I represents the difference in the amount of change in position, velocity, rotation, gyroscope bias and acceleration bias between the two frames of the object, V represents the reprojection error, i.e. PP, D, I, V It can also be called the four constraints of the objective function, that is, the four residual terms.

The state outside the sliding window can also provide certain constraint information. In order to obtain a more realistic constraint relationship, the state outside the sliding window is also taken into account (but it is not optimized). The constraint information is transformed into the prior distribution of the variables to be optimized, that is, PP in the least squares problem.

For obtaining the geometry of the space from the depth image and thus estimating the transformation matrix between the two frames, the iterative closest point (ICP) residual term D uses the point-to-surface distance as:

D=(T·P _j -P _i )·n _i

和位移参数

是两帧之间的运动参数。Among them, P _j and P _i are the matched 3D points in the above depth image, n _i is the normal vector corresponding to the previous frame, and the rotation parameter

and displacement parameters

is the motion parameter between two frames.

According to the calculation characteristics of the iterative closest point, the system adopts GPU to improve the calculation efficiency of the iterative closest point. Because the depth image similarity between two frames is high, in order to avoid unnecessary iterations, the number of iterations is limited to a maximum of 5.

For IMU constraint I, given two frames of IMU measurements (acceleration and angular velocity), IMU preintegration can calculate position, velocity, and rotation. So the system defines the IMU error term as the difference in the amount of change in position, velocity, rotation, gyroscope bias, and acceleration bias between two frames.

For the reprojection error V, as a visual constraint, and define it on the tangent plane of the unit sphere:

是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，

和

是P对应的切平面的任意两个正交基。对于投影与反投影

是根据针孔相机模型推导的从三维空间点投影到二维空间点的投影函数，定义为：in,

is the predicted value of the unit vector of the three-dimensional point after the feature point is transformed by the rigid body, P is the three-dimensional point obtained by the back-projection of the feature point by the pixel coordinates through the pinhole camera model,

and

are any two orthonormal bases of the tangent plane corresponding to P. For projection and back projection

is a projection function derived from a 3D space point to a 2D space point based on the pinhole camera model, and is defined as:

Among them, (f _x , f _y ) and (c _x , c _y ) are camera internal parameters, which can be obtained by camera internal parameter calibration methods such as checkerboard calibration method. The inverse operation of projection is backprojection.

In step S105 of pose estimation, an iterative method is used to solve the minimum value of the objective function to obtain the optimal pose of the object.

The objective function is formed by adding all the residual terms together, and the optimal pose can be obtained by minimizing the objective function using the iterative method. The iteration direction is determined by the first derivative of the objective function. In order to ensure the real-time performance of the system, the maximum number of iterations used to solve the pose is set to 8.

In one embodiment, before implementing the method, the state (gravity vector, gyroscope bias and velocity, etc.) on which the system depends is roughly estimated through a sliding window-based motion recovery structure (SfM) and a visual-inertial correction module. Methods provide initial values.

In one embodiment, the method further includes: a loop closure detection step S106 , detecting whether a loop closure occurs in the current frame, and if so, performing a global pose graph optimization to correct the positioning trajectory of the object to eliminate accumulated errors. This method adopts the loop closure detection based on the bag-of-words model. If the current frame is identified as a key frame, the system will convert the RGB image into a vector according to the dictionary. Then submit this vector to the loopback detection, and the loopback detection determines whether a loopback occurs in the current frame and returns the result. If a loopback is detected, a global Pose-Graph optimization will be performed to correct the positioning trajectory and eliminate accumulated errors.

In one embodiment, the least squares is solved based on a sliding window, the sliding window contains input parameters required for the calculation of the least squares, and the error term of each least squares except the prior information is determined by the sliding window. value provided. That is, the present invention adopts the form based on the sliding window to realize the least squares problem. The sliding window contains all the inputs (pixel points, three-dimensional space points, states, etc.) required for least squares, and the error term of each least squares is provided by the values in the sliding window (except for prior information). In order to limit the solution time of this least squares problem, the number of sliding windows is set to 10, which is enough to obtain an accurate estimation of the pose (also called pose) between two frames.

In one embodiment, the key frame in the RGB image is determined based on the following condition: the difference between the average disparity of the frame and the average disparity of the key frame of the previous frame exceeds a first threshold (for example, the first threshold is set to 1.0, in the The positioning performance is good in most scenes); or, the number of feature points tracked in this frame is less than the second threshold, for example, the second threshold is set to 10, which not only ensures the high robustness of the system but also does not generate too many key frames.

This method uses various sensors to collect RGB images, IMU measurement values and depth images of moving objects for attitude estimation, and constructs an objective function for attitude estimation. The objective function is solved by the least square method. The input value of the function is obtained by The sliding window is used for input, the specific number of sliding windows is set, the solution time is reduced, the estimation accuracy of the pose is improved, and the loopback detection process is also designed, and the global pose graph optimization is performed when a loopback occurs to correct the positioning of the object trajectory to eliminate the accumulated error, and specifically design the corresponding residual function.

Fig. 2 shows a three-dimensional attitude estimation device based on multi-sensor fusion of the present invention, the device includes:

The RGB image acquisition unit 201 is used to acquire the RGB image of the measured object using an image sensor, and extract the FAST feature point of the RGB image and use the KLT sparse optical flow method to track the feature point; the image sensor can be a camera, Cameras, etc., which can capture multiple frames of images of the object under test.

Whenever a new frame of RGB image arrives, the system will automatically extract FAST feature points, and then use the KLT sparse optical flow method to track the feature points. In order to ensure the accuracy of tracking and keep the computational cost relatively small, the number of feature points to be tracked can be set between 100 and 300, and new feature points will be detected during feature point tracking to maintain the number of tracked feature points. stay within a threshold.

The IMU measurement value acquisition unit 202 is configured to use the IMU sensor to collect the IMU measurement value of the measured object, and integrate the IMU measurement value to obtain the position, speed and rotation of the measured object at the current moment; the IMU sensor is also called inertial For the sensor, by integrating the discrete IMU measurement values, the position, speed and rotation at the current moment can be obtained, and the motion estimation of the object currently equipped with the sensor can be given.

The depth image acquisition unit 203 is used for acquiring the depth image of the measured object by using the depth image sensor, and using the iterative closest point (ICP) algorithm to process the depth image to obtain the iterative closest point; the depth image sensor may be a depth camera, In the depth camera, the spatial geometry of the measured object can be obtained through the depth image, thereby estimating the transformation matrix between two frames.

The objective function construction unit 204 is configured to construct an objective function based on the prior information, the iterative closest point, the IMU measurement value and the visual reprojection. Since each sensor has its own inapplicable environment, the environment during motion cannot be controlled by humans. In order to improve the robustness in multiple scenarios and obtain more accurate positioning, the present invention adopts multi-sensor data fusion to improve the accuracy and robustness of pose estimation.

The present invention comprehensively utilizes three kinds of sensor information such as RGB image, depth image, and IMU measurement value in a tightly-coupled manner to estimate the optimal attitude estimation between two frames, and solves the least squares problem in an iterative manner.

Specifically, the objective function is:

min _(R,t) {PP+∑||D|| ² +∑||I|| ² +∑||V|| ² };

和位移参数

使四个残差项PP、∑||D||²、∑||I||²与∑||V||²的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差，即PP、D、I、V也可以称为目标函数的四个约束，也就是四个残差项。A most suitable motion parameter is obtained by the least square method: rotation parameter

and displacement parameters

The value of the sum of the four residual terms PP, ∑||D|| ² , ∑||I|| ² and ∑||V|| ² is the lowest, where PP represents the prior distribution of prior information, D Represents the iterative nearest point residual term, I represents the difference in the amount of change in position, velocity, rotation, gyroscope bias and acceleration bias between the two frames of the object, V represents the reprojection error, i.e. PP, D, I, V It can also be called the four constraints of the objective function, that is, the four residual terms.

The state outside the sliding window can also provide certain constraint information. In order to obtain a more realistic constraint relationship, the state outside the sliding window is also taken into account (but it is not optimized). The constraint information is transformed into the prior distribution of the variables to be optimized, that is, PP in the least squares problem.

For obtaining the geometry of the space from the depth image and thus estimating the transformation matrix between the two frames, the iterative closest point (ICP) residual term D uses the point-to-surface distance as:

D=(T·P _j -P _i )·n _i

和位移参数

是两帧之间的运动参数。Among them, P _j and P _i are the matched 3D points in the above depth image, n _i is the normal vector corresponding to the previous frame, and the rotation parameter

and displacement parameters

is the motion parameter between two frames.

According to the calculation characteristics of the iterative closest point, the system adopts GPU to improve the calculation efficiency of the iterative closest point. Because the depth image similarity between two frames is high, in order to avoid unnecessary iterations, the number of iterations is limited to a maximum of 5.

For IMU constraint I, given two frames of IMU measurements (acceleration and angular velocity), IMU preintegration can calculate position, velocity, and rotation. So the system defines the IMU error term as the difference in the amount of change in position, velocity, rotation, gyroscope bias, and acceleration bias between two frames.

For the reprojection error V, as a visual constraint, and define it on the tangent plane of the unit sphere:

是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，

和

是P对应的切平面的任意两个正交基。对于投影与反投影

是根据针孔相机模型推导的从三维空间点投影到二维空间点的投影函数，定义为：in,

is the predicted value of the unit vector of the three-dimensional point after the feature point is transformed by the rigid body, P is the three-dimensional point obtained by the back-projection of the feature point by the pixel coordinates through the pinhole camera model,

and

are any two orthonormal bases of the tangent plane corresponding to P. For projection and back projection

is a projection function derived from a 3D space point to a 2D space point based on the pinhole camera model, and is defined as:

Among them, (f _x , f _y ) and (c _x , c _y ) are camera internal parameters, which can be obtained by camera internal parameter calibration methods such as checkerboard calibration method. The inverse operation of projection is backprojection.

The pose estimation unit 205 is configured to use an iterative method to solve the minimum value of the objective function to obtain the optimal pose of the object.

The objective function is formed by adding all the residual terms together, and the optimal pose can be obtained by minimizing the objective function using the iterative method. The iteration direction is determined by the first derivative of the objective function. In order to ensure the real-time performance of the system, the maximum number of iterations used to solve the pose is set to 8.

In one embodiment, before implementing the device, the state (gravity vector, gyroscope bias and velocity, etc.) on which the system depends is roughly estimated by a sliding window-based motion recovery structure (SfM) and a visual-inertial correction module. The device provides initial values.

In one embodiment, the apparatus further includes: a loop closure detection unit 206, configured to detect whether a loop closure occurs in the current frame, and if so, perform global pose graph optimization to correct the positioning trajectory of the object to eliminate accumulated errors . The device adopts the loop closure detection based on the bag-of-words model. If the current frame is identified as a key frame, the system will convert the RGB image into a vector according to the dictionary. Then submit this vector to the loopback detection, and the loopback detection determines whether a loopback occurs in the current frame and returns the result. If a loopback is detected, a global Pose-Graph optimization will be performed to correct the positioning trajectory and eliminate accumulated errors.

In one embodiment, the least squares is solved based on a sliding window, the sliding window contains input parameters required for the calculation of the least squares, and the error term of each least squares except the prior information is determined by the sliding window. value provided. That is, the present invention adopts the form based on the sliding window to realize the least squares problem. The sliding window contains all the inputs (pixel points, three-dimensional space points, states, etc.) required for least squares, and the error term of each least squares is provided by the values in the sliding window (except for prior information). In order to limit the solution time of this least squares problem, the number of sliding windows is set to 10, which is enough to obtain an accurate estimation of the pose (also called pose) between two frames.

In one embodiment, the key frame in the RGB image is determined based on the following condition: the difference between the average disparity of the frame and the average disparity of the key frame of the previous frame exceeds a first threshold (for example, the first threshold is set to 1.0, in the The positioning performance is good in most scenes); or, the number of feature points tracked in this frame is less than the second threshold, for example, the second threshold is set to 10, which not only ensures the high robustness of the system but also does not generate too many key frames.

The device collects RGB images, IMU measurement values and depth images of moving objects based on various sensors for attitude estimation, and constructs an objective function for attitude estimation. The objective function is solved by the least squares method, and the input value of the function is The sliding window is used for input, the specific number of sliding windows is set, the solution time is reduced, the estimation accuracy of the pose is improved, and the loopback detection process is also designed, and the global pose graph optimization is performed when a loopback occurs to correct the positioning of the object trajectory to eliminate the accumulated error, and specifically design the corresponding residual function.

For the convenience of description, when describing the above device, the functions are divided into various units and described respectively. Of course, when implementing the present application, the functions of each unit may be implemented in one or more software and/or hardware.

From the description of the above embodiments, those skilled in the art can clearly understand that the present application can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the technical solutions of the present application can be embodied in the form of software products in essence or the parts that make contributions to the prior art, and the computer software products can be stored in storage media, such as ROM/RAM, magnetic disks , CD, etc., including several instructions to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute the apparatus described in each embodiment or some part of the embodiment of the present application.

Finally, it should be noted that the above embodiments are only to illustrate rather than limit the technical solutions of the present invention. Although the present invention has been described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that: the present invention can still be modified or Equivalent replacements, and any modifications or partial replacements that do not depart from the spirit and scope of the present invention, shall all be included in the scope of the claims of the present invention.

Claims (12)

1. A three-dimensional attitude estimation method based on multi-sensor fusion is characterized by comprising the following steps:

an RGB image acquisition step, wherein an image sensor is used for acquiring an RGB image of a measured object, FAST characteristic points of the RGB image are extracted, and a KLT sparse optical flow method is used for tracking the characteristic points;

an IMU measurement value acquisition step, namely acquiring an IMU measurement value of a measured object by using an IMU sensor, and integrating the IMU measurement value to obtain the position, the speed and the rotation of the measured object at the current moment;

a depth image obtaining step, namely obtaining a depth image of the measured object by using a depth image sensor, and processing the depth image by using an Iterative Closest Point (ICP) algorithm to obtain an iterative closest point;

an objective function construction step, namely constructing an objective function based on prior information, an iteration closest point, an IMU (inertial measurement Unit) measurement value and visual reprojection;

and a pose estimation step, namely solving the minimum value of the objective function by using an iterative method to obtain the optimal pose of the object.

2. The method of claim 1, further comprising:

and a loop detection step, namely detecting whether a loop occurs in the current frame, if so, executing global pose graph optimization to correct the positioning track of the object so as to eliminate the accumulated error.

3. The method of claim 2, wherein the objective function is:

min_(R，t){PP+∑||D||²+∑||I||²+∑||V||²}；

the most suitable motion parameter is obtained by the least square method: rotation parameter

And displacement parameter

Making four residual terms PP, ∑ | | | D | | non-woven²、∑||I||²And ∑ V calculation²Where PP represents the prior distribution of prior information, D represents the iterative closest point residual term, I represents the difference in the amount of change in position, velocity, rotation, gyroscope and acceleration biases between two frames of the object, and V represents the reprojection error.

4. The method of claim 3, wherein the least squares are solved based on a sliding window, the sliding window containing input parameters required for the least squares calculation, and the error term of each least square, excluding the prior information, is provided by a value within the sliding window.

5. The method of claim 4, wherein the key frame in the RGB image is determined based on the following conditions:

the difference value of the average parallax of the frame and the average parallax of the key frame of the previous frame exceeds a first threshold value;

alternatively, the number of feature points tracked by the frame is less than a second threshold.

6. The method of claim 3,

the spatial geometry is derived from the depth image to estimate the transformation matrix between two frames, and the Iterative Closest Point (ICP) residual term D uses the point-to-face distance as:

D＝(T·P_j-P_i)·n_i

wherein, P_jAnd P_iIs a matched three-dimensional point, n, in the depth image_iIs the corresponding normal vector, rotation parameter of the previous frame

And displacement parameter

Is a motion parameter between two frames;

the reprojection error V serves as a visual constraint and is defined on the tangent plane of the unit sphere:

wherein,

is the predicted value of the unit vector of the three-dimensional point after the rigid body transformation of the characteristic point, P is the three-dimensional point obtained by the back projection of the pixel coordinate of the characteristic point through the pinhole camera model,

and

are any two orthogonal bases of the tangent plane to which P corresponds.

7. A three-dimensional attitude estimation device based on multi-sensor fusion is characterized by comprising:

an RGB image acquisition unit, which is used for acquiring an RGB image of a measured object by using an image sensor, extracting FAST characteristic points of the RGB image and tracking the characteristic points by using a KLT sparse optical flow method;

the IMU measurement value acquisition unit is used for acquiring an IMU measurement value of the measured object by using an IMU sensor and integrating the IMU measurement value to obtain the position, the speed and the rotation of the measured object at the current moment;

the depth image acquisition unit is used for acquiring a depth image of the measured object by using a depth image sensor and processing the depth image by using an Iterative Closest Point (ICP) algorithm to obtain an iterative closest point;

the objective function construction unit is used for constructing an objective function based on the prior information, the iteration closest point, the IMU measurement value and the visual re-projection;

and the pose estimation unit is used for solving the minimum value of the objective function by using an iterative method to obtain the optimal pose of the object.

8. The apparatus of claim 7, further comprising:

and the loop detection unit is used for detecting whether a loop occurs to the current frame, and if so, performing global pose graph optimization to correct the positioning track of the object so as to eliminate the accumulated error.

9. The method of claim 8, wherein the objective function is:

min_(R，t){PP+∑||D||²+∑||I||²+∑||V||²}；

the most suitable motion parameter is obtained by the least square method: rotation parameter

And displacement parameter

Making four residual terms PP, ∑ | | | D | | non-woven²、∑||I||²And ∑ V calculation²Where PP represents the prior distribution of prior information, D represents the iterative closest point residual term, I represents the difference in the amount of change in position, velocity, rotation, gyroscope and acceleration biases between two frames of the object, and V represents the reprojection error.

10. The method of claim 9, wherein the least squares are solved based on a sliding window, the sliding window containing input parameters required for the least squares calculation, and the error term of each least square, excluding the prior information, is provided by a value within the sliding window.

11. The method of claim 10, wherein determining the key frame in the RGB image is based on:

the difference value of the average parallax of the frame and the average parallax of the key frame of the previous frame exceeds a first threshold value;

alternatively, the number of feature points tracked by the frame is less than a second threshold.

12. The method of claim 9,

the spatial geometry is derived from the depth image to estimate the transformation matrix between two frames, and the Iterative Closest Point (ICP) residual term D uses the point-to-face distance as:

D＝(T·P_j-P_i)·n_i

wherein, P_jAnd P_iIs a matched three-dimensional point, n, in the depth image_iIs the corresponding normal vector, rotation parameter of the previous frame

And displacement parameter

Is a motion parameter between two frames;

the reprojection error V serves as a visual constraint and is defined on the tangent plane of the unit sphere:

wherein,

of unit vectors of three-dimensional points after rigid body transformation of characteristic pointsPredicting value P is three-dimensional point obtained by pixel coordinate back projection of pinhole camera model,

and

are any two orthogonal bases of the tangent plane to which P corresponds.

Images