CN105096341A - Mobile robot pose estimation method based on trifocal tensor and key frame strategy - Google Patents

Abstract

Aiming at the problem of visual servo tracking control, the invention proposes a mobile robot pose estimation method based on trifocal tensor and key frame strategy. For the generalized scene, the trifocal tensor is used to describe the geometric relationship among the initial view, the current view and the final view, and based on this, the pose information of the robot with unknown scale factors in the final view is obtained. For a wide range of visual servoing tasks, the key frame-based method estimates the pose of the robot without matching feature points between the current view, the initial view, and the final view, and obtains continuous pose measurements in global coordinates, greatly The working space of the robot visual servo has been expanded. The obtained pose information can be widely used in the design of visual servo controller.

Description

Pose Estimation Method for Mobile Robot Based on Trifocal Tensor and Key Frame Strategy

technical field

The invention belongs to the intersecting field of robot and computer vision, relates to the problem of pose estimation of a mobile robot based on vision, in particular to a pose estimation method of a mobile robot based on trifocal tensor and key frame strategy.

Background technique

With the rapid development of robot technology, robots play an important role in practice, and the tasks they undertake are more complex and diverse. In particular, for mobile robots, traditional methods are often based on position measurement devices such as GPS and odometers to control them. The control accuracy is limited by the accuracy of sensing devices, and the task flexibility is poor. The reference trajectory in space increases the difficulty and cost of implementation. Visual servo control uses visual information as feedback to measure the environment in a non-contact manner, utilizes a larger amount of information, improves the flexibility and accuracy of the robot system, and plays an irreplaceable role in robot control.

Classical visual servo control methods can be divided into methods based on position, image and geometric constraints, which are mainly reflected in the different construction methods of the error system. According to the previous review (Jia Bingxi, Liu Shan, Zhang Kaixiang, Chen Jian. Research Progress of Robot Visual Servo: Vision System and Control Strategy, Acta Automatica Sinica, 2015, 41(5):861-873), methods based on images and geometric constraints The robustness to image noise and camera calibration error is better, and requires less prior knowledge, and has a wider range of applications. However, for most visual servoing systems, it is generally required that the target feature points are at least partially within the field of view during the control process, so as to construct effective visual feedback. However, due to the limited field of view of the camera, and the existing image feature extraction methods are not repeatable and accurate when large rotation/translation occurs, which affects the accuracy of the visual servo system and limits its working space. In addition, for mobile robots, most mobile robots have nonholonomic constraints, that is, they have three spatial degrees of freedom, but only two control degrees of freedom. In previous studies, scholars (SalarisP, FontanelliD, PallottinoL, et al. Shortest paths for arobot with nonholonomic and field-of-view constraints, IEEE Transactions on Robotics, 2010, 26(2): 269-281) proposed a trajectory planning strategy to ensure that the target is visible for non-holonomic mobile robots. However, certain prior knowledge is required, and the planned path is often tortuous, which reduces the working efficiency of the robot in practice.

The multi-view geometric constraints commonly used in visual servoing mainly include homography, epipolar geometry, trifocal tensor, etc. Visual servoing based on multi-view geometry generally involves estimating the pose of the robot. Among them, homography-based visual servoing usually combines three-dimensional spatial information and image information to construct an error system, but there are multiple solutions (four sets of solutions) when decomposing the homography matrix, which often requires prior knowledge of the target plane. Knowledge, in order to solve the convenience, the feature points are often required to be coplanar; the method based on epipolar geometry uses the pole information between the two views to estimate the relative pose of the robot, but when the two views are close to each other, there will be a singularity, and when the two poles are both When adjusting to zero, there is ambiguity in the pose. It can only ensure that the two directions are the same and collinear. It is necessary to switch to other control strategies to adjust to the target pose. The method based on the trifocal tensor uses the corresponding relationship of the three views to estimate the pose of the robot, which has nothing to do with the structure of the scene, but only with the relative pose between the views, which has better universality. However, compared to homography and epipolar geometry, which are two-view geometry methods, the trifocal tensor requires matching feature points in three views, and has stronger requirements for field of view constraints. In addition, the repeatability and accuracy of the method of feature point extraction and matching are not good when rotation and translation transformation occur, which affects the accuracy of the visual servo system and limits the working space of the robot visual servo.

Contents of the invention

In order to overcome the deficiencies of previous technologies, the present invention proposes a mobile robot pose estimation method based on trifocal tensor and key frame strategy, and a visual servo tracking system and mobile robot based on the method.

A mobile robot pose estimation method based on the trifocal tensor and key frame strategy, which is used for the visual trajectory tracking task of the mobile robot. The environment image captured by the mobile robot during the teaching process is used as the expected trajectory, and the key points are selected from the expected trajectory. frame, and then construct the geometric model of the three views based on the trifocal tensor, thereby extracting the pose information; the three views are the current view C and any other two views C ₀ , C ^* .

The geometric model construction process is, for the current view C, by calculating the trifocal tensor with the other two views C ₀ , C ^* , so as to obtain the pose information of the current view relative to C ^* , including angle information and unknown The location information of the scale factor.

The image sequence containing the initial view and the final view selected from the expected trajectory is used as a key frame, and there is a matching degree of a preset threshold between adjacent key frames; in the process of pose estimation, first calculate the current view and The trifocal tensor between the two most matching keyframes is used to obtain the pose information relative to the most matching keyframe, and then iteratively transformed to the final view to obtain continuous pose measurements in global coordinates.

A visual servo tracking system employing any one of the methods.

A mobile robot employing the method according to any one.

Beneficial effects of the present invention:

For a wide range of visual servoing tasks, there is no need for matching feature points between the current view and the initial view and final view, thus greatly expanding the working space of robot visual servoing; The matching rate, so the calculation of the trifocal tensor in the keyframe-based pose estimation algorithm is more accurate, which improves the accuracy of the system.

Description of drawings

Figure 1 is a schematic diagram of the visual servo tracking task;

Fig. 2 is a schematic diagram of the trifocal tensor;

Figure 3 is a schematic diagram of pose estimation based on key frames;

Figure 4 is a schematic diagram of pose transformation between keyframes.

Detailed ways

The present invention will be described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

Aiming at the generalized scene, the present invention uses the trifocal tensor to describe the geometric relationship among the initial view, the current view and the final view, and obtains the pose information of the robot with an unknown scale factor based on it. Aiming at the problem of large-scale visual servo tracking, and making full use of the fact that the trifocal tensor has nothing to do with the environment structure, a pose estimation method based on keyframes is proposed. First, the pose information of the current view relative to the two most similar keyframes is calculated, and then The iterative transformation to the final view obtains continuous pose measurements in global coordinates. A mobile robot pose estimation method based on the trifocal tensor is mainly used for vision-based trajectory tracking problems. This type of problem is generally divided into two parts: teaching and tracking. As shown in Figure 1, in the teaching link, the robot takes images of the environment as the expected trajectory during the movement process; in the tracking link, the robot tracks the expected image based on visual feedback to complete the tracking of the expected trajectory. The tracking process is mainly divided into pose estimation and control two parts. The present invention proposes a pose estimation method including unknown scale factors, and then utilizes existing methods to easily design a corresponding adaptive controller to complete trajectory tracking tasks, such as (ChenJ, DixonWE, DawsonDM, et al.Homography-basedvisualservotrackingcontrolofawheeledmobilerobot[J] . Robotics, IEEE Transactions on, 2006, 22(2): 406-415.). For the pose estimation of the mobile robot, the present invention is mainly divided into two parts: geometric model construction and pose estimation based on key frames. 1. Geometric model

As shown in Figure 2, the trifocal tensor {T _i } _{i=1, 2, 3} describes the relationship among the three views, and has nothing to do with the environment structure. For the corresponding feature points x ₀ , x, x ^* in the three images I ₀ , I, I ^* , the relationship is as follows:

in, Respectively represent the i, j, k elements of x ₀ , x, x ^* , T _iqr represents the element of row q and column r in T _i , ε _jqs , ε _krt are arrangement symbols, defined as follows:

In particular, for a planar mobile robot, consider the relationship between the initial view C ₀ , the current view C, and the final view C ^* . A global coordinate system is established at C ^* , then the coordinates of the view C ₀ can be expressed as (x ₀ , z ₀ , θ ₀ ), and the coordinates of the current view can be expressed as (x, z, θ). Define R ₀ , t ₀ as the rotation and translation from C ₀ to C ^* , define R, t as the rotation and translation from C to C ^* , and the pose transformation relationship between the three views C, C ₀ , C ^* is as follows:

Express R, R ₀ as R=[r ₁ r ₂ r ₃ ],R ₀ =[r ₀₁ r ₀₂ r ₀₃ ], express t,t ₀ as t=[t _x 0t _z ] ^T ,t ₀ = [t _0x 0t _0z ] ^T , then the relationship between the trifocal tensor and the relative pose information is as follows:

After substituting the pose transformation relationship, the expression of the trifocal tensor about the relative pose between the three views can be obtained, where the non-zero elements are as follows, and T _ijk represents the element of row j and column k in T _i :

T ₁₁₁ ＝t _0x cosθ-t _x cosθ _n , T ₁₁₃ ＝t _0z cosθ+t _x sinθ ₀ ,

T ₁₃₁ ＝-t _0x sinθ-t _z cosθ ₀ , T ₁₃₃ ＝-t _0z sinθ+t _z sinθ ₀ ,

T ₂₁₂ =-t _x , T ₂₂₁ =-t _0x , T ₂₂₃ =t _0z , T ₂₃₂ =-t _z ,

T ₃₁₁ ＝t _0x sinθ-t _x sinθ ₀ , T ₃₁₃ ＝t _0z sinθ-t _x cosθ ₀ ,

T ₃₃₁ ＝t _0x cosθ-t _z sinθ ₀ , T ₃₃₃ ＝t _0z cosθ-t _z cosθ ₀ .

Based on images captured in three views, a trifocal tensor of unknown scale factor can be determined. definition is the distance from C ₀ to C ^* . In general, the coordinate origins of C ₀ and C ^* do not coincide, that is Perform the following normalization operations on the trifocal tensor:

Wherein α is a sign variable, which can be determined by sign(T ₂₂₁ )sign(t _0x ) or sign(T ₂₂₃ )sign(t _0z ). The signs of t _0x and t _0z do not change during the control process, and can be obtained by using prior knowledge in the teaching process.

According to the above derivation, define t _zm ＝t _x /d, t _zm ＝t _z /d as the position information of unknown scale factor, which can be calculated according to the normalized trifocal tensor:

The rotation angle θ of the robot can be determined by the following formula:

Define x _m ＝x/d, z _m ＝z/d as the coordinate information of unknown scale, which can be calculated by the following formula:

In the control system, (t _xm ,t _zm ,θ) or (x _m ,z _m ,θ) can be used to express the current pose information of the robot. In addition, given an image sequence, the expected trajectory can be calculated using the same method.

2. Keyframe-based pose estimation

As shown in Figure 3, the main idea of keyframe-based pose estimation is to select a series of keyframes in the desired trajectory, so that there are more matching points between adjacent keyframes. The pose information of the current frame is first calculated based on the two most similar key frames, and then iteratively transformed to obtain the pose information in the final view. The key frame strategy mainly includes two parts: key frame selection and pose estimation.

2.1 Key frame selection

The key frame selection process is given an image sequence {C _d } on the desired trajectory, and a series of key frames {C _k } are selected from it, so that the first key frame is the initial view, and the last key frame is the final view, The matching rate of two adjacent keyframes is close to the threshold τ. Among them, the matching rate between frame C _t and key frame C _p is defined as the ratio of the number of matching feature points in the two views to the number of feature points extracted in C _p . The selection process is as follows:

(1) Add C ₀ to {C _k }, i=1;

(2) Consider the next frame C _t in {C _d };

(3) If the matching rate between C _t and C _k (i) is less than τ, add C _t to {C _k }, i=i+1;

(4) If C _t is not C ^* , then return to (2);

(5) If C ^* is not in {C _k }, add C ^* to {C _k }.

2.2 Keyframe-based pose estimation

Given the current frame C, the idea of pose estimation is to first calculate the pose information relative to the two most similar key frames, and then iteratively transform to the final view C ^* , the specific process is as follows:

(1) Select the key frame C _k (i-1), C _k (i) most similar to the current frame based on the matching rate;

(2) Calculate the trifocal tensor T between C _k (i-1), C, and C _k (i);

(3) Calculate the pose information X _m (x _m,i ,z _m,i ,θ _i ) relative to C _k (i);

(4) For the key frames behind C _k (i) in {C _k }, iteratively transform the pose information until C ^* .

As shown in Figure 4, the attitude transformation method between two adjacent key frames is as follows:

(1) Calculate the trifocal tensor T of C _k (j-1), C _k (k+1), and C _k (j);

(2) Calculated according to T

(3) calculation

(4) calculation

In the above pose transformation, the trifocal tensor information between key frames can be pre-calculated offline to improve time efficiency. In addition, since there are more matching points between adjacent key frames, the calculation of the trifocal tensor is more accurate. And as the robot gets closer to the target, the cumulative error of pose transformation becomes smaller and smaller. In addition, the unknown scale factor in the global pose information measured based on the keyframe strategy is the distance between the last two keyframes, which remains unchanged during the robot's motion.

Claims (5)

1. A mobile robot pose estimation method based on trifocal tensor and key frame strategy is characterized by being used for a visual track tracking task of a mobile robot, utilizing an environment image shot by the mobile robot in a teaching process as an expected track, selecting a key frame from the expected track, and then carrying out geometric model construction on three views based on the trifocal tensor so as to extract pose information; the three views are a current view C and any two other views C₀,C^*。

2. The method of claim 1, wherein the geometric model is constructed by computing the current view C and the other two views C₀,C^*The trifocal tensor, thereby obtaining the current view relative to C^*The pose information of (1) includes angle information and position information including unknown scale factors.

3. The method according to claim 2, wherein an image sequence including an initial view and a final view selected from the desired trajectory is used as a key frame, and adjacent key frames have a matching degree of a preset threshold; in the pose estimation process, the trifocal tensor between the current view and the two most matched key frames is calculated to obtain pose information relative to the most matched key frames, and then the pose information is iteratively transformed to the final view to obtain continuous pose measurement under the global coordinate.

4. A visual servo tracking system employing the method according to any of claims 1-4.

5. A mobile robot employing the method according to any one of claims 1-4.