CN101667037B - Feasible channel-based robot target tracking method - Google Patents

Abstract

A kind of robot target tracking method based on Feasible channel of the present invention, is related to robot technology, and robot vision and code-disc information obtain location estimation of the target under robot coordinate system

And by under ultrasound and infrared sensor information unification to the coordinate system, decision space is generated, while obtaining distribution of obstacles point set

(k=0,1.., Kd-1); Establish robot channel set

(t=0,1,.., Kf-1), in conjunction with the target information and distribution of obstacles information in decision space, suitable robot channel is chosen, and then the infrared sensor information in integrated decision-making space, the rotational angle theta and velocity magnitude v for determining robot, realize the control to robot, robot are made to track target while avoiding obstacles. The information realization that the method for the present invention provides robot by itself vision, ultrasound, infrared sensor and code-disc touches tracking to the nothing of target. This method only makes a policy according to itself perception information, may be implemented to control in real time, for mobile robot Prevention-Security, in terms of using providing technical support.

Description

A kind of robot target tracking method based on Feasible channel

Technical field

The present invention relates to the mobile robot technology field, particularly a kind of robot target tracking method based on Feasible channel.

Background technology

Since first robot was born in the world, Robotics had obtained development rapidly.The range of application of robot has also expanded to a plurality of fields such as military affairs, nuclear industry, Aero-Space, service sector, medical treatment, human daily life from industry manufacture field.The mobile robot who wherein has broad prospect of application and military value has been subjected to common concern.

Target tracking is mobile robot's a cross-section study content, has important application prospects in tasks such as military affairs, anti-terrorism, security.After obtaining the position of target, the problem that needs solution is that the obstacle around how hiding also keeps the tracking to target.There are some to keep away barrier at present, march on towards the method for target, such as Artificial Potential Field method and improvement, based on the method for behavior, the application of intelligence control method etc., some progress have been obtained, but consider the factor such as uncertainty, complicacy of physical size, the actual environment of robot, robot obstacle-avoiding follows the trail of the objective still needs deep research.

Summary of the invention

The purpose of this invention is to provide a kind of robot target tracking method based on Feasible channel, the information that robot is provided by self vision sensor, sonac, infrared sensor and code-disc realizes the nothing of target is bumped tracking, reaches gratifying effect.

For achieving the above object, technical solution of the present invention is:

A kind of robot target tracking method based on Feasible channel is characterized in that, comprises step:

A) the colour code tube is set on target;

B) to determine target, comprehensive vision measurement and code-disc information obtain the estimated position of target under the robot coordinate system with the colour code tube on the video camera searching target in robot

C) with ultrasonic and infrared sensor information unification under the robot coordinate system, in conjunction with the target location of estimating, the decision space of generation robot obtains barrier distribution point set simultaneously $P_n^k(k=\mathrm{0,1},...,K_d-1);$

D) set up the robot channel set (t=0,1 ..., K _f-1);

E), select suitable machine people passage in conjunction with target information in the decision space and barrier distributed intelligence;

F) according to infrared sensor information in the decision space and robot channel selecting situation, determine the rotational angle theta and the velocity magnitude v of robot, realize ROBOT CONTROL.

Described a kind of robot target tracking method based on Feasible channel, the colour code tube on its described target is the cylindrical tube of hollow, is combined up and down by at least two kinds of colors, the center of colour code tube and the center of target are consistent.

Described a kind of robot target tracking method based on Feasible channel, its described robot coordinate are to be limit with the robot center, and current direction of motion is the polar coordinate system of pole axis.

Described a kind of robot target tracking method based on Feasible channel, the number K of its described barrier distribution point set _dIt is 4 multiple.

Described a kind of robot target tracking method based on Feasible channel, its described robot passage is an oriented rectangle, and the robot center is seated on this rectangle.

Described a kind of robot target tracking method based on Feasible channel, the number K of its described robot channel set _fBe 4 multiple, and K _dBe K _fMultiple.

The present invention only makes a policy according to self perception information, can realize real-time control, for the application of mobile robot at aspects such as Prevention-Security, securities provides technical support.

Description of drawings

Fig. 1 is the control block diagram of a kind of robot target tracking method based on Feasible channel of the present invention;

Fig. 2 is barrier distribution (K _d=16) synoptic diagram;

Fig. 3 is the tracing path of robot to static object;

Fig. 4 is the track that dynamic object is followed the trail of by robot.

Embodiment

The invention provides a kind of robot target tracking method based on Feasible channel, go out target by vision and code-disc data-evaluation behind the position in self coordinate system in robot, with sonac and the unification of infrared sensor information under the robot coordinate system, in conjunction with the target location of estimating, produce the decision space of robot, obtain barrier distribution point set simultaneously; Set up the robot channel set, in conjunction with target information in the decision space and barrier distributed intelligence, select suitable machine people passage, and then the infrared sensor information in the decision making package space, determine the corner and the velocity magnitude of robot, realization makes robot follow the trail of the objective in avoiding obstacles to ROBOT CONTROL.Control block diagram as shown in Figure 1, wherein,

Be the estimated position of target under the robot coordinate system, (k=0,1 ..., K _d-1) be barrier distribution point set,

(t=0,1 ..., K _f-1) be robot passage set, θ is the robot corner, and v is robot speed's size.

1. the target location is estimated

Foundation is limit with the robot center, and current direction is robot coordinate system's ∑ of pole axis.Follow robot four video camera S are arranged _v(i) (i=1,2,3,4), the line direction from the machine philtrum heart to these four video camera photocentres is consistent with optical axis direction separately respectively, and turns right with robot current direction of motion respectively Direction, current direction of motion, current direction of motion are turned left

The opposite direction of direction, current direction of motion is consistent.

Be equipped with a colour code tube on the target, the colour code tube is the cylindrical tube of hollow, is combined up and down by at least two kinds of colors, and the center of colour code tube and the center of target are consistent.Robot in conjunction with vision calibration, and then obtains the relative information of estimation by the visual identity of colour code tube is finished identification of targets.Order

Be the estimated position of target in robot coordinate system's ∑.

Definition (u v) is that image coordinate is fastened a bit, (x, y are its coordinates on world coordinate system z), have:

$\mathrm{<figure-callout\; id="1"\; label="z\; u\; v"\; filenames="H2008101195872E00012.png"\; state="\{\{state\}\}">z</figure-callout>}\left[\begin{array}{c}u\\ v\end{array}\right]\left[\begin{array}{c}\\ 1\end{array}\right]=M\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{\&alpha;}}_x& 0& u_0\\ 0& {\mathrm{\&alpha;}}_{\mathrm{<figure-callout\; id="0"\; label="y\; v"\; filenames="H2008101195872E00011.png,H2008101195872E00012.png"\; state="\{\{state\}\}">y</figure-callout>}}& v_{}{}_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

Wherein M is the intrinsic parameter matrix of video camera, is obtained by camera calibration.

Note (u ₁, v ₁) and (u ₂, v ₂) be respectively the point of colour code tube top and bottom, (x ₁, y ₁, z ₁) and (x ₂, y ₂, z ₂) be the coordinate of their pairing world coordinate systems, u ₁=u ₂, z ₁=z ₂, y ₂-y ₁Be the true altitude of colour code tube, then

Estimate as follows:

${\mathrm{\&rho;}}_n^T=z_1+d_c=z_2+d_c=\frac{{\mathrm{\&alpha;}}_y(y_2-y_1)}{v_2-{\mathrm{<figure-callout\; id="1"\; label="v"\; filenames="H2008101195872E00012.png"\; state="\{\{state\}\}">v</figure-callout>}}_1}+d_c$

α in the formula _yObtain v by M ₂-v ₁Be that the pixels tall of colour code tube in image is poor, d _cIt is the distance between video camera and the robot center.

Note (u _T, v _T) be the center of colour code tube, u _d, θ _vBe respectively the width of image and the width in the visual field, then

Estimate as follows:

${\mathrm{\&theta;}}_n^T=\left\{\begin{array}{cc}\mathrm{arc\; tan}(\frac{{2u}_T}{u_d}\&CenterDot;\tan ({\mathrm{\&theta;}}_v/2))+(i-2)\&CenterDot;\frac{\mathrm{\&pi;}}{2}& (i=\mathrm{1,2,3})\\ \mathrm{arc\; tan}(\frac{{2u}_T}{u_d}\&CenterDot;\tan ({\mathrm{\&theta;}}_v/2))+\mathrm{\&pi;}& (i=4^u_T<0)\\ \mathrm{arc\; tan}(\frac{{2u}_T}{u_d}\&CenterDot;\mathrm{ta}n({\mathrm{\&theta;}}_v/2))-\mathrm{\&pi;}& (i=4^u_T\&GreaterEqual;0)\end{array}\right.$

If robot can't find target, at this moment, vision measurement lost efficacy, and can adopt code-disc information estimating target position.If robot is at T _dCan't find still in time that it has to search for.Note

Be the coordinates of targets of last sampling instant, D _l, D _rBe respectively one decision-making period revolver and the distance exercised of right wheel.Thereby, the corner α of robot _nWith the robot center exercise apart from d _nEstimate as follows:

${\mathrm{\&alpha;}}_n=\mathrm{arc\; tan}\left(\frac{D_r-D_l}{d_{\mathrm{rl}}}\right),$ $d_n=\frac{D_l+D_r}{2}$

Wherein, d _RlBe the distance between revolver and the right wheel.

The coordinate of target in robot coordinate system's ∑ can followingly be estimated:

${\mathrm{\&rho;}}_n^T=\sqrt{{\mathrm{\&rho;}}_{n-1}^{T2}+d_n^2-2{\mathrm{\&rho;}}_{n-1}^T{d}_n\cos ({\mathrm{\&theta;}}_{n-1}^T-{\mathrm{\&alpha;}}_n)}$

${\mathrm{\&theta;}}_n^T={\mathrm{\&theta;}}_{n-1}^T-{\mathrm{\&alpha;}}_n+\mathrm{sgn}({\mathrm{\&theta;}}_{n-1}^T-{\mathrm{\&alpha;}}_n)\arccos \left(\frac{{\mathrm{\&rho;}}_{n-1}^{T2}+{\mathrm{\&rho;}}_n^{T2}-d_n^2}{2{\mathrm{\&rho;}}_n^T{\mathrm{\&rho;}}_{n-1}^T}\right)$

2. decision space

In the robot system of reality, different sensors is distributed in the diverse location of robot.For this reason, be necessary heat transfer agent unifiedly under robot coordinate system's ∑, obtain the decision space of robot, be convenient to robot and carry out decision-making based on sensing.Based on sonac information, the environment around the robot is divided into K _dEqual portions, K _dIt is 4 multiple.The robot peripheral obstacle distributes and can be described as point set

(k=0,1 ..., K _d-1), With

Definition set is described $\mathrm{\&zeta;}=\left\{P_n^k({\mathrm{\&rho;}}_n^k,{\mathrm{\&theta;}}_n^k)(k=\mathrm{0,1},...,K_d-1)\right\},$ See Fig. 2, wherein, ${\mathrm{\&theta;}}_n^k=\frac{2\mathrm{k\&pi;}}{K_d}-\mathrm{\&pi;},$

For in the decision space with

Corresponding sonac information.

3. robot passage

Consider that robot has the certain physical size, the robot channel definition is an oriented rectangle, and width is Dr _w, length is Dr _l, and the robot center is seated on this rectangle.

The setting robot is total $K_f=\frac{K_d}{C_{\mathrm{df}}}$ Individual passage can be selected, K _fBe 4 multiple, K _dBe K _fC _DfDoubly, C _Df0 be a natural number.These passages are formed channel set

(t=0,1 ..., K _f-1), corresponding direction of each passage ${\mathrm{\&theta;}}_{\mathrm{FP}}^t=-\mathrm{\&pi;}+\frac{2\mathrm{\&pi;}}{K_f}t,$ Usable set ξ is expressed as $\mathrm{\&xi;}=\left\{{\mathrm{FP}}_v^t\right|{\mathrm{\&theta;}}_{\mathrm{FP}}^t=-\mathrm{\&pi;}+\frac{2\mathrm{\&pi;}}{K_f}t(t=\mathrm{0,1},...,K_f-1)\},$ With the robot corresponding passage of current direction of motion be forward path

Definition barrier distribution point set

Subclass ${\mathrm{\&Omega;}}_t=\left\{P_n^s({\mathrm{\&rho;}}_n^s,{\mathrm{\&theta;}}_n^s)(s=({\mathrm{tC}}_{\mathrm{df}}+j)\mathrm{mod}{K}_d)\right|j\&Element;[-\frac{K_d}{4},\frac{K_d}{4}]\}.$

Definition

Point is described

With passage

Relation, when this point is in passage, $S_t^k=1$ 。So have,

$S_t^k=\left\{\begin{array}{cc}1& \left(\right|{\mathrm{\&rho;}}_n^k\sin \left((k-{\mathrm{tC}}_{\mathrm{df}})\frac{2\mathrm{\&pi;}}{K_d}\right)|<\frac{{\mathrm{Dr}}_w}{2})^\left(\right|{\mathrm{\&rho;}}_n^k\cos \left((k-{\mathrm{tC}}_{\mathrm{df}})\frac{2\mathrm{\&pi;}}{K_d}\right)|<{\mathrm{Dr}}_l)\\ 0& \mathrm{others}\end{array}\right.$

Work as Ω _tIn all points do not exist

In, mean passage

Be safe, robot can be along passage

Direction motion, note $F_n^t=0.$

$F_n^t=\left\{\begin{array}{cc}1& S_t^k=1,\&Exists;P_n^k\&Element;{\mathrm{\&Omega;}}_{\mathrm{<figure-callout\; id="0"\; label="t"\; filenames="H2008101195872E00011.png,H2008101195872E00012.png"\; state="\{\{state\}\}">t</figure-callout>}}\\ 0& \mathrm{others}\end{array}\right.$

4. choose suitable machine people passage

In order to realize the effective tracking of robot, need from all safe passages, select suitable one to target.Choice criteria is that robot moves towards target direction as far as possible, and definition suitable machine people passage is

Definition robot motion direction to the angle of target direction is

${\mathrm{\&theta;}}_n^g=-{\mathrm{\&theta;}}_n^T+{2k}_g\mathrm{\&pi;},$ k _g＝0(n＝0)， ${\mathrm{\&theta;}}_0^g=-{\mathrm{\&theta;}}_0^T$ 。Wherein, ${\mathrm{\&theta;}}_n^T\&Element;[-\mathrm{\&pi;},\mathrm{\&pi;})$ $,{\mathrm{\&theta;}}_n^g\&Element;(-\&infin;,\&infin;).$ When Around-π or the π during saltus step, k _gValue change. From just bearing k _gAdd 1;

From just to negative, k _gSubtract 1.

At first calculate the passage at target place

$T=[\frac{(\mathrm{\&pi;}+{\mathrm{\&theta;}}_n^T)}{2\mathrm{\&pi;}}\&CenterDot;K_f].$

If $(-\mathrm{\&pi;}<{\mathrm{\&theta;}}_{n-1}^T<-\mathrm{\&pi;}/2)^(\mathrm{\&pi;}/2<{\mathrm{\&theta;}}_n^T<\mathrm{\&pi;}),$ K is then arranged _g=k _g+ 1; If $(\mathrm{\&pi;}/2<{\mathrm{\&theta;}}_{n-1}^T<\mathrm{\&pi;})^(-\mathrm{\&pi;}<{\mathrm{\&theta;}}_n^T<-\mathrm{\&pi;}/2),$ K then _g=k _g-1.

(1) if ${\mathrm{\&theta;}}_n^g\&GreaterEqual;0$ , judge whether the respective channel of current direction is feasible, promptly

Whether be 0.If $F_n^{K_f/2}=0$ , search i=0,1 ... up to (K _f/ 2-i) mod K _f=T makes $F_n^{{(K_f/2-i)\mathrm{mod}K}_f}=1,$ So, p=(K _f/ 2-i+1) mod K _fIf

Be always 0, p=T.If $F_n^{K_f/2}=1,$ Search i=0,1 ..., up to (K _f/ 2+i) mod K _f=T makes $F_n^{{(K_f/2+i)\mathrm{mod}K}_f}=0,$ So p=(K _f/ 2+i) mod K _fIf Be always 1, show and do not find suitable passage.

(2) if ${\mathrm{\&theta;}}_n^g<0$ , judge whether the respective channel of current direction is feasible, promptly

Whether be 0.If $F_n^{K_f/2}=0$ , search i=0,1 ... up to (K _f/ 2+i) mod K _f=T makes $F_n^{{(K_f/2+i)\mathrm{mod}K}_f}=1$ So, p=(K _f/ 2+i-1) mod K _fIf, Be always 0, p=T.If $F_n^{K_f/2}=1$ , search i=0,1 ... up to (K _f/ 2-i) mod K _f=T makes $F_n^{{(K_f/2-i)\mathrm{mod}K}_f}=0,$ So p=(K _f/ 2-i) mod K _fIf

Be always 1, showing does not have suitable passage.

5. robot decision-making

In case robot obtains , robot calculates corner $\mathrm{\&theta;}=-\mathrm{\&pi;}+\frac{2\mathrm{\&pi;}}{K_f}p$ If the robot pairing infrared sensor of current direction of motion detects obstacle, robot speed's size v is made as 0, otherwise, v=v _dWhen robot can't find suitable passage, v got 0.

Embodiment

A kind of robot target tracking method based on Feasible channel provided by the present invention is applied on the intelligent robot AIM of Chinese Academy of Sciences's robotization Research Institute, this diameter 48cm by the differential driving of two-wheel, is equipped with 4 video cameras, No. 16 sonacs, No. 16 infrared sensors.About 60 ° of each camera coverage.When the distance between robot and the target during, think and follow the trail of successfully less than 60cm.K _d＝160，C _df＝4，K _f＝40，Dr _l＝100cm，Dr _w＝60cm，T _d＝5，d _rl＝0.27m，d _c＝5cm，v _d＝20cm/s。Adopt target tracking method provided by the present invention, effect is satisfactory, and when Fig. 3 and Fig. 4 had provided the static and target travel of target respectively, the nothing of robot was bumped tracing path.

Claims (4)

1. the robot target tracking method based on Feasible channel is characterized in that, comprises step:

A) the colour code tube is set on target;

B) robot with the colour code tube on the video camera searching target determining target, comprehensive vision measurement and code-disc information, obtaining target is being limit with the robot center, current direction is the estimated position under the coordinate system of pole axis

C) with ultrasonic and infrared sensor information unification under the robot coordinate system, the estimated position of combining target produces the decision space of robot, obtains barrier distribution point set simultaneously

D) set up robot Feasible channel collection

E), select suitable machine people Feasible channel in conjunction with target information in the decision space and barrier distributed intelligence;

F) select situation according to infrared sensor information in the decision space and robot Feasible channel, determine the rotational angle theta and the velocity magnitude v of robot, realize ROBOT CONTROL;

The number K of described barrier distribution point set _dIt is 4 multiple;

The number K of described robot Feasible channel collection _fBe 4 multiple, and K _dBe K _fMultiple.

2. a kind of robot target tracking method as claimed in claim 1 based on Feasible channel, it is characterized in that, colour code tube on the described target is the cylindrical tube of hollow, is combined up and down by at least two kinds of colors, and the center of colour code tube and the center of target are consistent.

3. a kind of robot target tracking method based on Feasible channel as claimed in claim 1 is characterized in that, described robot coordinate is to be limit with the robot center, and current direction of motion is the polar coordinate system of pole axis.

4. a kind of robot target tracking method based on Feasible channel as claimed in claim 1 is characterized in that described robot Feasible channel is an oriented rectangle, and the robot center is seated on this rectangle.

Images