CN105955271A - Multi-robot coordinated motion method based on multi-view geometry - Google Patents

Abstract

The present invention discloses a multi-robot coordinated motion method based on multi-view geometry. The method comprises the following steps of appointing two robots in a multi-robot system as pilot machines and other robots as following machines; calculating the two-focus tensors between the following machine and the two pilot machines to obtain two projection straight lines of the target position of the following machine on an image, wherein the straight line intersection point is the projection point of the target position on a following machine image; enabling the following machine to move along a connection line of an optical center of a camera image of the following machine and the target position projection point until the positions of the projection points of the pilot machines are overlapped with the projection positions before motion, repeating the above processing on the residual following machines one by one, thereby realizing the situation that the relative positions of the residual following machines and the two pilot machines are consistent with the relative positions before motion. According to the present invention, the multi-view geometry is applied to the multi-robot system, the mutual projection knowledge of a plurality of cameras is introduced, the control stability is improved, and the action of the multi-view geometry in the robot control is promoted.

Description

A kind of multi-robot coordination movement technique based on multiple views geometry

Technical field

The present invention relates to technical field of robot control, be specifically related to a kind of multi-robot coordination based on multiple views geometry Movement technique.

Background technology

Multi-robot coordination is moved to have in real life and is widely applied very much.On the one hand, some work is only with one Robot has been difficult to, and just can well be achieved the goal by the cooperation of multiple robots；On the other hand, multirobot is passed through Between coordination, robot system efficiency in operation process can be improved.In a lot of applications, it is desirable that many machines People's coordination exercise, especially in the case of completing the tasks such as military affairs, production, fire-fighting, also requires robot manipulating task and is moved through Journey keeps certain formation, how to control robot and keep rank and seem even more important.Here it is the formation of robot controls.

In research and the application of multi-robot coordination operation, present existing control method, due to deficient in stability, Robot can only be to compare in indoor or level land etc. to realize coordination exercise in the most single environment, strongly limit the use of robot Scope and application conditions, be difficult to realize the co-ordination of multirobot in more universal complicated landform.

Summary of the invention

Goal of the invention: for the deficiencies in the prior art, the present invention provides a kind of multirobot based on multiple views geometry to assist Adjust movement technique, improve the stability of control method so that robot is applicable to more complicated complete dynamic environment.

Technical scheme: multi-robot coordination movement technique based on multiple views geometry of the present invention, for multirobot System, comprises the steps:

(1) specify the Liang Tai robot in multi-robot system as guide's machine, remaining robot as the machine of following, two Guide's machine first moves；

(2) choose a machine of following and be designated as C₁, two guide's machines are designated as C respectively₂And C₃, C₁、C₂、C₃Position before motion is divided It is not designated as C₁ ^t、C₂ ^t、C₃ ^t, C₂And C₃First moving, post exercise position is respectively C₂ ^t+1、C₃ ^t+1；

(3) C is calculated₁ ^tWith C₂ ^t+1Between bifocal tensor F₁₂, utilize formula l₂=F₁₂e₂₁ ^tCalculate C₁Target location C₁ ^t+1 At C₁Projection straight line l on image₂, wherein antipodal points e₂₁ ^tIt is C₁ ^tAt C₂ ^tProjection on image；

(4) C is calculated₁ ^tWith C₃ ^t+1Between bifocal tensor F₁₃, utilize formula l₃=F₁₃e₃₁ ^tCalculate C₁Target location C₁ ^t+1 At C₁Projection straight line l on image₃, wherein antipodal points e₃₁ ^tIt is C₁ ^tAt C₃ ^tProjection on image, projection straight line l₃With step (3) Gained projection straight line l₂Intersection point x be C₁Target location C₁ ^t+1At C₁Subpoint on image；

(5) C is made₁Optical center and C along its camera review₁The line motion of target location subpoint x, until C₂、C₃ At C₁Subpoint position e on camera review₁₂、e₁₃With the projected position e before motion₁₂ ^t、e₁₃ ^tOverlap, i.e. realize C₁、C₂、C₃ Relative position before post exercise moves relative to position keeps consistent, completes one and follows machine and two guide's machines coordination fortune The control that dynamic formation keeps；

(6) machine of following residue repeats step (2) to (5) one by one, it is achieved residue follows the relative of machine and two guide's machines Relative position before position moves keeps consistent, thus completes all robot coordinated motion formations of multi-robot system and protect The control held.

Improving technique scheme further, in described step (3), step (4), the algorithm of bifocal tensor is as follows:

X, x ' it is spatial point X subpoint on the two width planes of delineation, bifocal tensorElement be determinantal expansion X ' in formula^jxⁱCoefficient, be written as:

Wherein, aⁱ、bⁱIt is the spatial point X row vector that projects in projection matrix A and B that x, x on two width images ' puts respectively； To r, s, t=1,2,3, tensor ε_rstIt is defined as follows:

To given i value, unless p, q are different from i and different, otherwise tensor ε_ipqIt is zero, to ε_jrsIn like manner；

As long as from the foregoing, it will be observed that provide corresponding point, often group corresponding point will provide for 1 bilinear relation formula: then byBy x₁WritingCan obtain after expansion:

$\left[\begin{array}{ccccccccc}{\mathrm{uu}}^{\′}& {\mathrm{vu}}^{\′}& u^{\′}& {\mathrm{uv}}^{\′}& {\mathrm{vv}}^{\′}& v^{\′}& u& v& 1\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ \·\\ \·\\ \·\\ f_{33}\end{array}\right]=0;$

In the case of two viewpoints, provide 8 groups of corresponding point, then can obtain following system of linear equations:

$\left[\begin{array}{ccccccccc}{u}_1{u}_1^{\′}& v_1{u}_1^{\′}& u_1^{\′}& u_1{v}_1^{\′}& v_1{v}_1^{\′}& v_1^{\′}& u_1& v_1& 1\\ \·& & & & & & & & \·\\ \·& & & & & & & & \·\\ \·& & & & & & & & \·\\ u_8{u}_8^{\′}& v_8{u}_8^{\′}& u_8^{\′}& u_8{v}_8^{\′}& v_8{v}_8^{\′}& v_8^{\′}& u_8& v_8& 1\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ \·\\ \·\\ \·\\ f_{\mathrm{33}}\end{array}\right]=\left[\begin{array}{c}0\\ \·\\ \·\\ \·\\ 0\end{array}\right];$

Above-mentioned homogeneous equation group,It is identified the poorest invariant, when order is 8, it is possible to use linear algorithm to ask Obtain unique solution.

Further, in described step (3), step (4), the algorithm of bifocal tensor is as follows:With

Define a vector t, vector t be byFormed: in the case of two viewpoints, In the case of three viewpoints,In the case of four viewpoints,If having N number of Corresponding point, have following relation: Mt=0, M are N*9 respectively, the matrix of 9N*27,81N*81；

Assume to obtain N from the antipodal points provided₁Individual Line independent relational expression, M₁T=0, obtains N from corresponding point₂Individual line Property independence formula, M₂They simultaneous are become the system of a linear relation by t=0:At M₁Middle addition 0 OK, square formation M' is createed₁, from SVD (M'₁)=UDV^TIn extract V, and by leaving out the front N of V₁Row obtain V', fromIn solve t', finally obtain result t from t=V't'.

Further, described step (5) is until C₂And C₃At C₁ ^tE on image₁₂、e₁₃With the projected position e before motion₁₂ ^t、 e₁₃ ^tEuclidean distance both less than equal to 5pixel time, i.e. realize C₁、C₂、C₃Post exercise move relative to position before relative Position keeps consistent.

Beneficial effect: compared with prior art, advantages of the present invention: multiple views geometric techniques applies to automatically control motion There have been vicennial history, such as three-dimensional reconstruction, vision induction etc. in field, is widely used in the automatic control of multiple robot System, the automatic Pilot of automobile, camera chain, Motion Recognition etc., but traditional multiple views geometry is mostly applied to static state Environment, the most applicable for dynamic environment.Present invention firstly provides new multiple views geometry, introduce multiple cameras and mutually throw The knowledge of shadow, the research of lifting multiple views geometry is to higher field, and its stability has compared to conventional multi-view geometry Large increase, promotes the effect in robot control of the multiple views geometry so that robot is applicable to more complicated complete dynamic ring Border, solves the technical barriers such as stability well.Can manufacture, based on this theory, the robot that the suitability is higher, meet people's day The productive life requirement that benefit increases.

Accompanying drawing explanation

Fig. 1 is the schematic diagram utilizing bifocal tensor to realize robot coordinated motion.

Detailed description of the invention

Below by accompanying drawing, technical solution of the present invention is described in detail.

Embodiment 1: multi-robot coordination movement technique based on multiple views geometry of the present invention, including walking as follows Rapid:

(1) using the Liang Ge robot in multi-robot system as guide's machine, remaining robot, as the machine of following, first examines Considering a coordination exercise method followed between machine and guide's machine, the machine of following is designated as C₁, two guide's machines are designated as C respectively₂And C₃；

Position before (2) three robot motions is respectively C₁ ^t、C₂ ^t、C₃ ^t, post exercise position is designated as C₁ ^t+1、C₂ ^t+1、C₃ ^{t +1}, when guide's machine moves to C₂ ^t+1、C₃ ^t+1Time, follow machine also at initial position C₁ ^t, as it is shown in figure 1, determine C by subsequent calculations₁ ^{t +1}Position；

(3) C is calculated₁ ^tAnd C₂ ^t+1Between bifocal tensor F₁₂, can get C according to multiple views geometry₁ ^t+1On image Projection straight line l₂, computing formula is l₂=F₁₂e₂₁ ^t, wherein antipodal points e₂₁ ^tIt is C₁ ^tAt C₂ ^tOn projection, e₂₁ ^tIt is used for remembering movement Front formation；

(4) same to step (3), calculate C₁And C₃ ^t+1Between bifocal tensor F₁₃, utilize bifocal tensor sum antipodal points e₃₁ ^t Between relational expression l₃=F₁₃e₃₁ ^t, calculate C₁ ^t+1At C₁ ^tOn projection straight line l₃, e₃₁ ^tIt is C₁ ^tAt C₃ ^tOn projection, integrating step (3) projection straight line l obtained₂, the intersection point X of two straight lines, namely destination locations C can be obtained₁ ^t+1At C₁ ^tProjection on image；

(5) machine C is followed in order₁Along C₁ ^tMove with the line of X, until C₂And C₃At C₁ ^tE on image₁₂With e₁₃Position and e₁₂ ^tWith e₁₂ ^tThe Euclidean distance of position both less than equal to 5pixel, to realize C₁ ^t+1、C₂ ^t+1、C₃ ^t+1Relative position and C₁ ^t、 C₂ ^t、C₃ ^tPosition keep consistent, i.e. complete the control that robot coordinated motion formation is kept, in like manner other robot is also Can " follow up " in this way.

Wherein bifocal tensor F₁₂And F₁₃Algorithm as follows:

(1) corresponding point method:

X, x ' it is spatial point X subpoint on the two width planes of delineation, bifocal tensorElement be determinantal expansion X ' in formula^jxⁱCoefficient.Can be written as:

Wherein, aⁱ、bⁱIt is the spatial point X row vector that projects in projection matrix A and B that x, x on two width images ' puts respectively； To r, s, t=1,2,3, tensor ε_rstIt is defined as follows:

ε_ipq、ε_jrsDefinition and ε_rstIdentical, therefore for given i value, unless p, q are different from i and different, otherwise open Amount ε_ipqIt is zero, to ε_jrsIn like manner.Therefore this and formula only comprise four nonzero terms.And come across the determinant in these four all Comprise the four same row of matrix A and B, thus they have equal value except outer symbol.But, ε_ipqε_jrsValue make this Four have identical symbol equal.

As long as by upper it is known that provide corresponding point, often group corresponding point will provide for 1 bilinear relation formula.Then byBy x₁WritingCan obtain after expansion:

$\left[\begin{array}{ccccccccc}{\mathrm{uu}}^{\′}& {\mathrm{vu}}^{\′}& u^{\′}& {\mathrm{uv}}^{\′}& {\mathrm{vv}}^{\′}& v^{\′}& u& v& 1\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ \·\\ \·\\ \·\\ f_{33}\end{array}\right]=0.$

If providing 8 groups of match points, then can obtain following system of linear equations:

$\left[\begin{array}{ccccccccc}{u}_1{u}_1^{\′}& v_1{u}_1^{\′}& u_1^{\′}& u_1{v}_1^{\′}& v_1{v}_1^{\′}& v_1^{\′}& u_1& v_1& 1\\ \·& & & & & & & & \·\\ \·& & & & & & & & \·\\ \·& & & & & & & & \·\\ u_8{u}_8^{\′}& v_8{u}_8^{\′}& u_8^{\′}& u_8{v}_8^{\′}& v_8{v}_8^{\′}& v_8^{\′}& u_8& v_8& 1\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ \·\\ \·\\ \·\\ f_{\mathrm{33}}\end{array}\right]=\left[\begin{array}{c}0\\ \·\\ \·\\ \·\\ 0\end{array}\right]$

This is a homogeneous equation group,The poorest invariant can be identified.If there is a solution, the left side Rank of matrix is necessarily up to 8, if order is just 8, then solution is unique, and can solve with linear algorithm.

It can be seen that in the case of two viewpoints, needed for calculating the linear solution of multiple views geometry, corresponding point number is 8。

(2) mutual sciagraphy

If there being the projection of second video camera on piece image, it is possible to obtain the antipodal points of approximation, be denoted as e₁₂, In like manner can define e₂₁.The two antipodal points and fundamental matrix have a following relation:

With

Each of which relation all can provide three linear restrictions.But, the two linear relationship is not separate , if by two relation simultaneous, the most only five linear restrictions can be provided.

When there being two video cameras, have two kinds of projection situations:

(1) only have 1 video camera to be projected on another video camera；

(2) 2 video cameras mutually project.

Can obtain 1 antipodal points during situation (1), this antipodal points can provide 3 independent linearity constraints.

Can obtain 2 antipodal points during situation (2), these 2 antipodal points can provide 5 linear independent relations constraints.

Therefore, in order to calculate bifocal tensor, when not considering mutually to project, need 8 corresponding point.Considering mutually During projection, in the case of the first, have only to 5 corresponding point, in the case of the second, have only to 3 corresponding point.

A kind of method introducing now practicality, can calculate multifocal tensor by linear gauge in the case of mutually projection.

Define a vector t, vector t be byFormed: in the case of two viewpoints, In the case of three viewpoints,In the case of four viewpoints,If having N number of Corresponding point, have following relation: Mt=0, M are N*9 respectively, the matrix of 9N*27,81N*81；

Assume to obtain N from the antipodal points provided₁Individual Line independent relational expression, M₁T=0, obtains N from corresponding point₂Individual line Property independence formula, M₂They simultaneous are become the system of a linear relation by t=0:

${\[M_1^T,M_2^T\]}^{T}t=0$

If there being than requiring more linear relation in this system, just having a conventional method and going to find out A young waiter in a wineshop or an inn takes advantage of solution.Least square solution can make algebraic distance minimum, i.e.But this solution can not strictly meet right The constraint that limit is given.From the results of view, the antipodal points calculated from t by the method for least square of standard can not be given Antipodal points corresponding.In order to solve this problem, find that method of least square willIt is minimised as M₁T=0.Very Different value is decomposed and can is effectively used in here.Specifically, by M₁Middle addition 0 row, creates square formation M'₁.From SVD(M'₁)=UDV^TIn extract V, and by leaving out the front N of V₁Row obtain V'.FromIn solve t'.? Result t is obtained eventually from t=V't'.

Although as it has been described above, represented and described the present invention with reference to specific preferred embodiment, but it must not be explained For the restriction to the present invention self.Under the spirit and scope of the present invention premise defined without departing from claims, can be right Various changes can be made in the form and details for it.

Claims (4)

1. a multi-robot coordination movement technique based on multiple views geometry, for multi-robot system, it is characterised in that: bag Include following steps:

(1) specify the Liang Tai robot in multi-robot system as guide's machine, remaining robot as the machine of following, two guides Machine first moves；

(2) choose a machine of following and be designated as C₁, two guide's machines are designated as C respectively₂And C₃, C₁、C₂、C₃Position before motion is remembered respectively For C₁ ^t、C₂ ^t、C₃ ^t, C₂And C₃First moving, post exercise position is respectively C₂ ^t+1、C₃ ^t+1；

(3) C is calculated₁ ^tWith C₂ ^t+1Between bifocal tensor F₁₂, utilize formula l₂=F₁₂e₂₁ ^tCalculate C₁Target location C₁ ^t+1At C₁ Projection straight line l on image₂, wherein antipodal points e₂₁ ^tIt is C₁ ^tAt C₂ ^tProjection on image；

(4) C is calculated₁ ^tWith C₃ ^t+1Between bifocal tensor F₁₃, utilize formula l₃=F₁₃e₃₁ ^tCalculate C₁Target location C₁ ^t+1At C₁ Projection straight line l on image₃, wherein antipodal points e₃₁ ^tIt is C₁ ^tAt C₃ ^tProjection on image, projection straight line l₃With step (3) gained Projection straight line l₂Intersection point x be C₁Target location C₁ ^t+1At C₁Subpoint on image；

(5) C is made₁Optical center and C along its camera review₁The line motion of target location subpoint x, until C₂、C₃At C₁ Subpoint position e on camera review₁₂、e₁₃With the projected position e before motion₁₂ ^t、e₁₃ ^tOverlap, i.e. realize C₁、C₂、C₃Motion After relative position move before relative position keep consistent, complete one and follow machine and two guide machine coordination exercise teams The control that shape keeps；

(6) machine of following residue repeats step (2) to (5) one by one, it is achieved residue follows the relative position of machine and two guide's machines Relative position before moving keeps consistent, thus completes what all robot coordinated motion formations of multi-robot system kept Control.

Multi-robot coordination movement technique based on multiple views geometry the most according to claim 1, it is characterised in that: described step Suddenly in (3), step (4), the algorithm of bifocal tensor is as follows:

X, x ' it is spatial point X subpoint on the two width planes of delineation, bifocal tensorElement be in determinantal expansion x′^jxⁱCoefficient, be written as:

Wherein, aⁱ、bⁱIt is the spatial point X row vector that projects in projection matrix A and B that x, x on two width images ' puts respectively；ε_ipq、 ε_jrsIt is defined as: to r, s, t=1,2,3, tensor ε_rstFor:

To given i value, unless p, q are different from i and different, otherwise tensor ε_ipqIt is zero, to ε_jrsIn like manner；

As long as from the foregoing, it will be observed that provide corresponding point, often group corresponding point will provide for 1 bilinear relation formula: then by By x₁WritingCan obtain after expansion:

$\left[\begin{array}{ccccccccc}{\mathrm{uu}}^{\′}& {\mathrm{vu}}^{\′}& u^{\′}& {\mathrm{uv}}^{\′}& {\mathrm{vv}}^{\′}& v^{\′}& u& v& 1\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ \begin{array}{c}.\\ .\\ .\end{array}\\ f_{33}\end{array}\right]=0;$

In the case of two viewpoints, provide 8 groups of corresponding point, then can obtain following system of linear equations:

$\left[\begin{array}{ccccccccc}{u}_1{u}_1^{\′}& v_1{u}_1^{\′}& u_1^{\′}& u_1{v}_1^{\′}& v_1{v}_1^{\′}& v_1^{\′}& u_1& v_1& 1\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ u_8{u}_8^{\′}& v_8{u}_8^{\′}& u_8^{\′}& u_8{v}_8^{\′}& v_8{v}_8^{\′}& v_8^{\′}& u_8& v_8& 1\end{array}\right]\left[\begin{array}{c}{f}_{11}\\ f_{12}\\ f_{13}\\ .\\ .\\ .\\ f_{33}\end{array}\right]=\left[\begin{array}{c}0\\ .\\ .\\ .\\ 0\end{array}\right];$

Above-mentioned homogeneous equation group,It is identified the poorest invariant, when order is 8, it is possible to use linear algorithm to try to achieve only One solves.

Multi-robot coordination movement technique based on multiple views geometry the most according to claim 1, it is characterised in that: described step Suddenly in (3), step (4), the algorithm of bifocal tensor is as follows:With

Define a vector t, vector t be byFormed: in the case of two viewpoints,Three regard In the case of Dian,In the case of four viewpoints,If there being N number of correspondence Point, has following relation: Mt=0, M are N*9 respectively, the matrix of 9N*27,81N*81；

Assume to obtain N from the antipodal points provided₁Individual Line independent relational expression, M₁T=0, obtains N from corresponding point₂Individual linear only Vertical relational expression, M₂They simultaneous are become the system of a linear relation by t=0:At M₁Middle addition 0 row, Create square formation M'₁, from SVD (M'₁)=UDV^TIn extract V, and by leaving out the front N of V₁Row obtain V', fromIn solve t', finally obtain result t from t=V't'.

Multi-robot coordination movement technique based on multiple views geometry the most according to claim 1, it is characterised in that: described step Suddenly until C in (5)₂And C₃At C₁ ^tE on image₁₂、e₁₃With the projected position e before motion₁₂ ^t、e₁₃ ^tEuclidean distance both less than etc. When 5pixel, i.e. realize C₁、C₂、C₃Relative position before post exercise moves relative to position keeps consistent.