CN105096341A

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

Info

Publication number: CN105096341A
Application number: CN201510445644.6A
Authority: CN
Inventors: 陈剑; 贾丙西; 张凯祥
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2015-07-27
Filing date: 2015-07-27
Publication date: 2015-11-25
Anticipated expiration: 2035-07-27
Also published as: CN105096341B

Abstract

The invention provides a mobile robot pose estimation method based on a trifocal tensor and a key frame strategy aiming at a visual servo tracking control problem. Aiming at a general scene, the trifocal tensor is used to describe a geometrical relationship among an initial view, a current view and a final view. Based on this, pose information of the robot containing an unknown proportion factor under the final view is acquired. For a visual servo task with a large scope, based on a key frame method, a pose of the robot is estimated. A matching characteristic point existing among the current view, the initial view and the final view is not needed and continuous pose measurement under a global coordinate is obtained. A visual servo work space of the robot is greatly expanded. The obtained pose information can be widely used for a design of a visual servo controller.

Description

Mobile robot pose estimation method based on trifocal tensor and key frame strategy

Technical Field

The invention belongs to the crossing field of robot and computer vision, relates to a mobile robot pose estimation problem based on vision, and particularly relates to a mobile robot pose estimation method based on trifocal tensor and key frame strategies.

Background

With the rapid development of the robot technology, the robot plays an important role in practice, and the tasks undertaken by the robot are more complicated and diversified. Particularly, for a mobile robot, the conventional method is usually based on position measuring equipment such as a GPS and a odometer to control the mobile robot, the control accuracy of the method is limited by the accuracy of sensing equipment, the flexibility of the task is poor, a reference track of the robot in a physical space needs to be given, and the implementation difficulty and cost are increased. The vision servo control utilizes the vision information as feedback to carry out non-contact measurement on the environment, utilizes larger information quantity, improves the flexibility and the accuracy of the robot system, and has irreplaceable effect in the robot control.

Classical visual servo control methods can be divided into methods based on position, image and geometric constraints, which are mainly reflected by different error system construction modes. According to the previous research progress of the robot vision servo in the aspects of vision system and control strategy, automatic science report 2015,41(5):861-873, the image and geometric constraint-based method has better robustness to image noise and camera calibration errors, needs less prior knowledge and has wider application range. 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 control, in order to construct effective visual feedback. However, since the field of view of the camera is limited, and the existing image feature extraction method is not good in repeatability and accuracy when large rotation/translation occurs, the accuracy of the visual servo system is affected, and the working space of the visual servo system is limited. In addition, most mobile robots have incomplete constraints for them, i.e. three degrees of spatial freedom, but only two degrees of control freedom. In the past, researchers (Salarisp, FontaneliD, Pallotino L, et. Shortestpath for object with non-holonomic field-of-viewconstraints, IEEETransactionson Robotics,2010,26(2):269-281) proposed a trajectory planning strategy for ensuring that the target is visible for the mobile robot with incomplete constraints, but required a certain priori knowledge, and the planned path is often tortuous, which actually reduces the working efficiency of the robot.

The commonly used multi-view geometric constraints in visual servoing mainly include homography, epipolar geometry, trifocal tensor and the like, and the multi-view geometric-based visual servoing generally involves estimation of the pose of a robot. The homography-based visual servo usually combines three-dimensional spatial information and image information to construct an error system, but a multi-solution problem (four groups of solutions) exists when a homography matrix is decomposed, prior knowledge of a target plane is often needed, and feature points are often required to be coplanar for solving convenience; the method based on epipolar geometry estimates the relative posture of the robot by using the pole information between two views, but the two views are adjacent to each other, and the two views have singularity when the two poles are adjusted to zero, so that the two views can only be ensured to be identical in direction and collinear, and the robot needs to be switched to other control strategies to be adjusted to the target pose. The method based on the trifocal tensor estimates the posture of the robot by utilizing the corresponding relation of the three views, is irrelevant to the structure of a scene, is only relevant to the relative posture between the views, and has better universality. However, compared to the two-view geometry approach of homography and epipolar geometry, the trifocal tensor requires matching feature points in the three views, and imposes stronger constraints on the field of view. In addition, the repeatability and precision of the method for extracting and matching the feature points are not good when rotation and translation transformation occurs, the precision of a visual servo system is influenced, and the working space of the visual servo of the robot is limited.

Disclosure of Invention

Aiming at overcoming the defects of the prior art, the invention provides a mobile robot pose estimation method based on the trifocal tensor and the key frame strategy, a visual servo tracking system and a mobile robot according to the method, aiming at the problem of visual servo tracking control.

A mobile robot pose estimation method based on trifocal tensor and key frame strategy is used for a visual track tracking task of a mobile robot and is shot by the mobile robot in a teaching processSelecting a key frame from the expected track by taking the environment image as the expected track, and then constructing a geometric model of the 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^*。

The geometric model construction process is that for the current view C, the current view C is calculated and two other views C are used₀,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.

Selecting an image sequence containing an initial view and a final view from the expected track as key frames, wherein the 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.

A visual servo tracking system employing a method according to any one of the methods.

A mobile robot employing the method according to any one of the preceding claims.

The invention has the beneficial effects that:

aiming at a large-range visual servo task, matched feature points among a current view, an initial view and a final view are not needed, so that the working space of the visual servo of the robot is greatly expanded; because the matching rate close to the preset threshold value is formed between the adjacent key frames, the calculation of the trifocal tensor in the pose estimation algorithm based on the key frames is more accurate, and the accuracy of the system is improved.

Drawings

FIG. 1 is a schematic diagram of a visual servo tracking task;

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

FIG. 3 is a schematic diagram of keyframe based pose estimation;

FIG. 4 is a schematic diagram of pose transformation between key frames.

Detailed Description

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

Aiming at a generalized scene, the invention describes the geometric relationship among an initial view, a current view and a final view by using the trifocal tensor, and obtains the pose information of the robot containing unknown scale factors based on the geometric relationship. Aiming at the problem of large-scale visual servo tracking, the characteristic that the trifocal tensor is irrelevant to the environment structure is fully utilized, a key frame-based attitude estimation method is provided, the attitude information of the current view relative to two most similar key frames is calculated, then the pose information is iteratively transformed to the final view, and continuous attitude measurement under the global coordinate is obtained. A mobile robot pose estimation method based on trifocal tensor is mainly used for the track tracking problem based on vision. The problems are generally divided into two links of teaching and tracking. As shown in fig. 1, in the teaching link, the robot takes an image of the environment as an expected track during the movement; in the tracking link, the robot tracks the expected image based on visual feedback so as to complete the tracking of the expected track, and the tracking process mainly comprises two parts, namely pose estimation and control. The invention provides a pose estimation method containing unknown scale factors, and then the prior method is utilized to easily design a corresponding self-adaptive controller to complete a track tracking task, such as (ChenJ, DixonWE, DawsonDM, equivalent. Homography-based visual tracking control of electromagnetic mobile robot [ J ]. Robotics, IEEETransactionson,2006,22(2): 406-). 415.). Aiming at the pose estimation of the mobile robot, the pose estimation method mainly comprises two parts of geometric model construction and pose estimation based on key frames. 1. Geometric model

For instance, three jiao Zhang in FIG. 2Quantity { T }_i}_i＝1,2,3The interrelationship between the three views is described and is independent of the environment structure. For three images I₀,I,I^*Of (2) corresponding feature point x₀,x,x^*Having the following relationship:

wherein,respectively represent x₀,x,x^*I, j, k elements of (1), T_iqrRepresents T_iThe elements of the q-th row and the r-th column,_jqs,_krtto rank symbols, the following are defined:

in particular, for a planar mobile robot, consider the initial view C₀Current view C, and final view C^*The relationship (2) of (c). At C^*Establishing a global coordinate system, then view C₀Can be expressed as (x)₀,z₀,θ₀) The coordinates of the current view may be expressed as (x, z, θ). Definition of R₀,t₀Is C₀To C^*Rotation and translation of (1), definition R, t is C to C^*Rotation and translation of (1), three views C, C₀,C^*The pose transformation relationship between the two is as follows:

r, R₀Is represented by R ═ R₁r₂r₃],R₀＝[r₀₁r₀₂r₀₃]Let t, t₀Is expressed as t ═ t_x0t_z]^T,t₀＝[t_0x0t_0z]^TThen, the trifocal tensor and the relative pose information have the following relationship:

substituting the pose transformation relation to obtain an expression of the relative pose of the trifocal tensor with respect to the three views, wherein the nonzero elements are as follows, T_ijkRepresents T_iRow j and column k:

T₁₁₁＝t_0xcosθ-t_xcosθ_n,T₁₁₃＝t_0zcosθ+t_xsinθ₀,

T₁₃₁＝-t_0xsinθ-t_zcosθ₀,T₁₃₃＝-t_0zsinθ+t_zsinθ₀,

T₂₁₂＝-t_x,T₂₂₁＝-t_0x,T₂₂₃＝t_0z,T₂₃₂＝-t_z,

T₃₁₁＝t_0xsinθ-t_xsinθ₀,T₃₁₃＝t_0zsinθ-t_xcosθ₀,

T₃₃₁＝t_0xcosθ-t_zsinθ₀,T₃₃₃＝t_0zcosθ-t_zcosθ₀.

based on the images taken under the three views, the trifocal tensor for the unknown scale factor can be determined. Definition ofIs C₀To C^*The distance of (c). In general, C₀And C^*Do not coincide with the origin of coordinates of (i.e. do not coincide with each other)The trifocal tensor is normalized as follows:

where α is a sign variable, may be represented by sign (T)₂₂₁)sign(t_0x) Or sign (T)₂₂₃)sign(t_0z) And (4) determining. During control t_0x,t_0zThe sign of (A) is not changed, and can be obtained by using priori knowledge in the teaching process.

From the above derivation, define t_zm＝t_x/d,t_zm＝t_zThe/d is the position information of the unknown scale factor, and can be calculated according to the normalized trifocal tensor to obtain:

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

definition of x_m＝x/d,z_mZ/d is coordinate information of unknown proportion, which can be calculated by the following formula:

in the control system, (t) may be used_xm,t_zmθ) or (x)_m,z_mAnd theta) expresses the current pose information of the robot. In addition, given a sequence of images, the desired trajectory can be calculated using the same method.

2. Keyframe based pose estimation

As shown in fig. 3, the key frame-based pose estimation main idea is to select a series of key frames in the expected trajectory, so that there are more matching points between adjacent key frames. The pose information of the current frame is calculated based on the two most similar key frames, and then iterative transformation is carried out to obtain the pose information under the final view. The key frame strategy mainly comprises two parts of key frame selection and pose estimation.

2.1 Key frame selection

The key frame selection process is given as a sequence of images C on the desired trajectory_dSelect a series of key frames C_kAnd (4) enabling the first key frame to be an initial view, enabling the last key frame to be a final view, and enabling the matching rate of two adjacent key frames to be close to the threshold value tau. Wherein a frame C is defined_tAnd key frame C_pThe matching rate between the two views is the number of matched characteristic points in the two views and C_pThe ratio of the number of feature points is extracted. The selection process is as follows:

(1) c is to be₀Adding { C_k},i＝1；

(2) Consider { C_dThe next frame C in_t；

(3) If C is present_tAnd C_k(i) Is less than tau, C_tAdding { C_k},i＝i+1；

(4) If C is present_tIs not C^*And then returning to the step (2);

(5) if C is present^*Out of { C_kIn the front, thenC is to be^*Adding { C_k}。

2.2 keyframe-based pose estimation

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

(1) selecting a key frame C most similar to the current frame based on the matching rate_k(i-1),C_k(i)；

(2) Calculating C_k(i-1),C,C_k(i) The trifocal tensor T in between;

(3) relative to C_k(i) Position and orientation information X of_m(x_m,i,z_m,i,θ_i)；

(4) For { C_kIn C_k(i) The subsequent key frame is subjected to iterative pose information transformation until C^*。

As shown in fig. 4, the pose transformation method between two adjacent key frames is as follows:

(1) calculating C_k(j-1),C_k(k+1),C_k(j) The trifocal tensor T;

(2) calculating according to T

(3) Computing

(4) Computing

In the posture transformation, the trifocal tensor information among the key frames can be calculated off line in advance, so that the time efficiency is improved. In addition, the trifocal tensor is computed more accurately because there are more matching points between adjacent keyframes. And as the robot is closer to the target, the accumulated error of pose transformation is smaller. In addition, the unknown scale factor in the global pose information obtained based on the key frame strategy measurement is the distance between the last two key frames, and the unknown scale factor is kept unchanged in the robot motion process.

Claims

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.