CN100458625C - Underwater bionic robot cooperated transportation method - Google Patents

Abstract

The invention relates to a cooperation transport method of underwater bionic robot, comprising (1), state control based on limit ring theory, path design based on conform circle method, (3), synchronous control based on fuzzy assertion, (4), transport direction control based on fuzzy assertion. The invention uses the state control method based on limit ring method, flexibly controls the speed and direction of the underwater robot, to confirm the state of the robot is converged to the target state to resolve the state control problem. The path design method can reduce the robot control complexity in complex underwater condition and simplify the dimension of program space. And the motion design method based on fuzzy assertion can effectively avoiding the underwater robot to impact transport object via inertia. Therefore, the invention realizes synchronous control of robot and the stable control on the direction of transport object.

Description

A kind of underwater bionic robot cooperated transportation method

Technical field

The present invention relates to a kind of multirobot cooperated transportation method, particularly about the cooperated transportation method of underwater bionic robot.

Background technology

Along with the development of society, the robot application field is constantly expansion, to the daily life field, explores space operation field from ocean resources from automatic production line, and robot is ubiquitous.Yet, with regard to present Robotics level, single robot obtains, handles in information, and aspects such as control ability, decision-making adaptability to changes exist significant limitation, especially in complex work task and dynamic environment, the executive capability of single robot is more inadequate.To the transportation of hazardous material, in complex road surface or relative narrow space hauling operation the time, single robot need integrate various systems such as navigational system, delivery system, communication system.The integrated system development difficulty of this height is very big, and cost of development is than higher, and the construction cycle is also long, the more important thing is that the unit device people volume of integrated each system is huger, and it is dumb to take action.In a single day transportation system of single robot breaks down and just can't finish the work.Transportation system of single robot need develop at particular task, and every kind of new task all needs to develop again specific transportation robot, and the time of exploitation is long, and difficulty is bigger.

Along with land resources reduces day by day, ocean resources more and more are subject to people's attention.The exploration of ocean resources and the research of investigation aspect under water are just in expansion like a raging fire, and the transportation of various hydrospace detection equipment installation machine is under water finished.But transport under water to compare with land transport its distinctive difficult point is arranged: 1, the interference ratio in the water is bigger, so the antijamming capability of transportation system's control algolithm is under water had higher requirement, the cooperation algorithm of land robot can not be applied directly on the underwater robot, needs to propose control method new, that antijamming capability is stronger; 2, because friction force is less under water, the braking capacity of underwater robot is restricted, and is easy to overshoot.New control method will consider to brake this link specially, makes the control method design difficult more; 3, the underwater robot sport efficiency is low present stage, and energy dissipation is serious, and development of new underwater robot efficiently is imperative with the long working that adapts under water.

Summary of the invention

Singularity at underwater robot control is difficult to disturb problems such as big, that uncertain factor is many in foundation, the water with complicacy and Hydrodynamic Model, the purpose of this invention is to provide a kind of brand-new underwater bionic robot cooperated transportation method in complicated underwater environment.

For achieving the above object, the present invention takes following technical scheme: a kind of cooperated transportation method of underwater bionic robot, and it may further comprise the steps:

(1) based on the Pose Control of " limit cycle " principle

Be the point of contact with the impact point, be that tangential direction is made a circle of contact with the target direction, so promptly can determine center of circle B (x ₀, y ₀) the position.Represent the linear velocity of robot under original coordinate system with v, represent travel direction, can obtain with α:

$v=\sqrt{{\stackrel{\·}{\stackrel{\‾}{x}}}^2+{\stackrel{\·}{\stackrel{\‾}{y}}}^2}---\left(1\right)$

$\mathrm{\α}=\arctan \frac{\stackrel{\·}{\stackrel{\‾}{y}}}{\stackrel{\·}{\stackrel{\‾}{x}}}---\left(2\right)$

Wherein

$\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}=\mathrm{\&lambda;}(\stackrel{\&OverBar;}{y}-{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="C20071006475400141.png,C20071006475400191.png"\; state="\{\{state\}\}">y</figure-callout>}}_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{x}-x_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2))---\left(3\right)$

$\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}=\mathrm{\&lambda;}(-\stackrel{\&OverBar;}{x}+x_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{y}-y_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2))$

γ, λ are positive parameter, and r is the radius of the circle of contact, by the movement velocity and the direction of (1) (2) (3) formula control robot, can realize the Pose Control of robot.

(2) based on the path planning of " comfortable circle " method

(a) to decision tree input target position information, camera collection to visual information, the position that comprises robot and transportation thing and directional information etc.;

(b) generating one group by decision tree repels and complete " situation " of having described problem space mutually;

(c) corresponding to various " situations ", the behavior of design robot makes it under the restriction of minimal curve radius, moves to object pose along a suitable path;

(d) draw the path from robot location to the target location according to " comfortable circle " rules, and distribute the role of each robot in the cooperation transportation according to the cost evaluation function;

(3) based on the synchro control of fuzzy reasoning

With WL (i), WR (j) (being respectively the cost that i of robot and the j of robot distribute left comer look and right corner look) be expressed as fuzzy set L, M, S}, representative is big respectively, in, little; With speed VL, the VR of robot be expressed as fuzzy set F, M, S}, respectively representative fast, middling speed and at a slow speed, design following fuzzy rule:

1) if WL (i) is that L and WR (j) are L, VL is F so, and VR is F;

2) if WL (i) is that L and WR (j) are M, VL is F so, and VR is M;

3) if WL (i) is that L and WR (j) are S, VL is F so, and VR is S;

4) if WL (i) is that M and WR (j) are L, VL is M so, and VR is F;

5) if WL (i) is that M and WR (j) are M, VL is F so, and VR is F;

6) if WL (i) is that M and WR (j) are S, VL is F so, and VR is S;

7) if WL (i) is that S and WR (j) are L, VL is S so, and VR is F;

8) if WL (i) is that S and WR (j) are M, VL is S so, and VR is F;

9) if WL (i) is that S and WR (j) are S, VL is M so, and VR is M,

Use the inference mode of Mamdani type, the final speed of robot is obtained by " gravity model appoach " deblurring, and expression formula is as follows:

$\stackrel{\&OverBar;}{\mathrm{VL}}={\mathrm{\&Sigma;}}_{k=1}^9{\mathrm{VL}}_k{x}_k/{\mathrm{\&Sigma;}}_{k=1}^9{x}_k,$ $\stackrel{\&OverBar;}{\mathrm{VR}}={\mathrm{\&Sigma;}}_{k=1}^9{\mathrm{VR}}_k{x}_k/{\mathrm{\&Sigma;}}_{k=1}^9{x}_k---\left(4\right)$

x _k＝min{x _1k1，x _2k2} (5)

In the formula of (4) (5),, x _kBe k bar rule " if " part the simultaneous degree (k=1 ..., 9), x _1k1(correspondingly, x _2k2) be the degree of membership of WL (i) (correspondingly, WR (j)) to k bar rule, VL _kWith VR _kBe the output that obtains from k bar rule.

(4) control based on the transporting direction of fuzzy reasoning

With the chest direction with respect to the angle θ of target direction with fuzzy set PB, PM, PS, Z, NS, NM, NB} represent, and represent respectively honest, the center, just little, zero, negative little, negative in, negative big; With speed VL, the VR of robot be expressed as fuzzy set F, M, S}, respectively representative fast, middling speed and at a slow speed, design following fuzzy inference rule:

1) if θ is PB, VL is F so, and VR is S;

2) if θ is PM, VL is M so, and VR is S;

3) if θ is PS, VL is M so, and VR is S;

4) if θ is Z, VL is M so, and VR is M;

5) if θ is NS, VL is S so, and VR is M;

6) if θ is NM, VL is S so, and VR is M;

7) if θ is NB, VL is S so, and VR is F,

Adopt the inference mode of Mamdani type, the final speed of robot is obtained by " gravity model appoach " deblurring equally.

Adopt following cost function F (A) to estimate role assignments:

F(A)＝|WL(i)-WR(i)|+k(WL(i)+WR(j))，

WL(i)＝len(LP，P _i)+C×NL _obj，

WR(j)＝len(RP，P _j)+C×NR _obj.

P wherein _iAnd P _jRepresent the position that i of robot and j are current respectively, LP and RP represent left impact point and right impact point respectively, len (LP, P _i) (correspondingly, len (RP, P _j)) expression according to " comfortable circle " method planning from P _iPoint (correspondingly, P _jPoint) to the path of LP point (correspondingly, RP point).If from P _iArrive LP (correspondingly, from P _jTo RP) planning the path on have obstacle, NL _Obj(correspondingly, NR _Obj) be 1; Otherwise, be 0; WL (i) and WR (j) are respectively the cost that the i of robot and the j of robot are assigned as left comer look and right corner look.

The present invention is owing to take above technical scheme, it has the following advantages: 1, owing to the posture control method that has adopted based on " limit cycle " method, by speed and the direction of controlling underwater robot cleverly, can theoretically and can both guarantee in fact that the pose of robot converges to object pose, thereby solve the Pose Control problem of underwater robot.2, owing to the paths planning method that has adopted based on " comfortable circle ", thereby reduce the complexity that robot controls in the complicated underwater environment, and simplified the dimension of problem space.Also consider the maneuverability of underwater robot in addition, therefore the path according to the method planning all is feasible under arbitrary speed.3, owing to the motion planning method that has adopted based on fuzzy reasoning, thereby many in uncertain factor, as to disturb the accurate control of underwater robot under big environment difficult points have been removed effectively, prevented underwater robot owing to the bump of action of inertia, thereby realized steady control the object of transport direction to object of transport.

Description of drawings

Fig. 1 is quick convergent limit cycle synoptic diagram

Fig. 2 is the limit cycle synoptic diagram of slow convergence

Fig. 3 is a machine fish Pose Control synoptic diagram

Fig. 4 decision tree synoptic diagram

Fig. 5 is SYN situation and the corresponding behavior synoptic diagram thereof that obtains according to " situation-behavior " method

Fig. 6 is NFAD situation and the corresponding behavior synoptic diagram thereof that obtains according to " situation-behavior " method

Fig. 7 is NSYN situation and the corresponding behavior synoptic diagram thereof that obtains according to " situation-behavior " method

Fig. 8 is FP situation and the corresponding behavior synoptic diagram thereof that obtains according to " situation-behavior " method

Fig. 9 is NFP situation and the corresponding behavior synoptic diagram thereof that obtains according to " situation-behavior " method

Figure 10 is the fuzzy logic controller synoptic diagram

Figure 11 is WL in the fuzzy control, the subordinate function synoptic diagram of WR

Figure 12 is VL in the fuzzy control, the subordinate function synoptic diagram of VR

Figure 13 be in the fuzzy control object direction with respect to the subordinate function synoptic diagram of target direction angle

Figure 14 is a multimachine device fish cooperation platform synoptic diagram

Figure 15 is a multimachine device fish cooperation transportation synoptic diagram

Embodiment

Below in conjunction with drawings and Examples, the present invention will be described in detail.

The present invention's transportation problem of will cooperating is divided into four subproblems and solves: 1, Pose Control; 2, path planning; 3, synchro control; 4, transporting direction control.Describe as example with bionic machine fish below.

1, based on the Pose Control of " limit cycle " method

In the second nonlinear autonomous system, have a class important by isolated motion of closing the path statement, not only theoretical own to the differential equation, and on engineering is used, also play a part very important, the isolated rail that closes of this class is called " limit cycle ", the point that sets out in the optional position all converges to " limit cycle ", and does periodically along it and to move.Owing to adopt classical control method can not guarantee that underwater robot arrives impact point with correct pose, and didactic method can't guarantee accurate Pose Control theoretically, therefore the present invention is applied to " limit cycle " theory in the Pose Control of machine fish, by speed and the direction of controlling underwater robot cleverly, can make the robot location converge to limit cycle and move, thereby successfully solve the Pose Control problem of underwater robot along limit cycle.

With

(x ₀, y ₀) represent some A and the planimetric coordinates of some B under two-dimensional coordinate system respectively, consider following nonlinear system:

$\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}=\mathrm{\&lambda;}(\stackrel{\&OverBar;}{y}-{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="C20071006475400141.png,C20071006475400191.png"\; state="\{\{state\}\}">y</figure-callout>}}_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{x}-x_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2)),---\left(1\right)$

$\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}=\mathrm{\&lambda;}(-\stackrel{\&OverBar;}{x}+x_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{y}-y_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2)),$

γ wherein, λ, r are positive parameter.We can provide following theorem:

Theorem 1:

If in a nonlinear system, point

Motion can describe with (1) formula, this system has one with B (x so ₀, y ₀) be that the center of circle, form are ${(\stackrel{\&OverBar;}{x}-x_0)}^2+{(\stackrel{\&OverBar;}{y}-y_0)}^2=r^2$ Circle " limit cycle ", all converge to this " limit cycle " from the point of optional position, and do periodic motion around it.

In equation (1), point

Speed of convergence to limit cycle can be regulated by parameter γ, as shown in Figure 1 and Figure 2, has provided the limit cycle under the different parameters γ.Fig. 1 is quick convergent situation (γ=0.001), and Fig. 2 is the situation (γ=0.0003) of slow convergence.

The mode of motion that the machine fish takes equation (1) to describe, its position will inevitably converge to " limit cycle " of a circle and move along it, and the tangential direction of " limit cycle " of the object pose of machine fish can be by machine fish arrival impact point the time realizes.The posture control method of machine fish is as follows: be the point of contact with the impact point, be that tangential direction is made a circle of contact with the target direction, so promptly can determine center of circle B (x ₀, y ₀) the position.By " limit cycle " method, the machine fish can be realized the object pose of appointment, and promptly impact point is that point of contact, target direction are the tangential direction (as shown in Figure 3) of the circle of contact through impact point.For the final control that realizes the machine fish, the present invention is converted into the motion control of equation (1) statement speed and the direction control of machine fish.Represent the linear velocity of machine fish under original coordinate system with v, represent travel direction, can obtain with α:

$v=\sqrt{{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}}^2+{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}}^2}=\mathrm{\&lambda;}\sqrt{{\tilde{x}}^2+{\tilde{y}}^2},---\left(2\right)$

$\mathrm{\&alpha;}=\arctan \frac{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}}{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}}=\frac{\tilde{y}}{\tilde{x}}.$

In (2), in (3) formula, $\tilde{x}=\stackrel{\&OverBar;}{y}-{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="C20071006475400141.png,C20071006475400191.png"\; state="\{\{state\}\}">y</figure-callout>}}_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{x}-x_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2),$

$\tilde{y}=-\stackrel{\&OverBar;}{x}+x_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{y}-y_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2)---\left(3\right)$

From (2) formula as can be known, regulate the value of parameter lambda, the size that just can regulate v arbitrarily.The machine fish is taked (2), and the movement technique of (3) formula statement promptly can be realized its Pose Control.

2, based on the path planning of " comfortable circle " method

In order to reduce the complexity of underwater robot control, and simplify the dimension of problem space as far as possible, the present invention has adopted " situation-behavior " method based on " comfortable circle " of a kind of novelty to solve path planning problem.

Consider the difficulty of the true motion control of machine milt, the present invention reduces the complexity of path planning in the cooperation transportation with the method for designing of " situation-behavior "." situation-behavior " method is based on the method for designing of definition one group " situation ", and these " situations " have described the relative status between the problem entity.This method for designing meets the following conditions: a) repel and intactly represented relative status between the problem entity between " situation " mutually; And " situation " space of definition can not be excessive; B) each " situation " corresponding behavior can be independently solved relevant problem.The advantage of " situation-behavior " method of use is: itself be the strategy of a kind of " cutting apart ", therefore reduced the complexity of task; In addition, the problem that need the consideration behavior merge because " situation " constituted complete state space, and is repelled mutually.Realize-" comfortable circule method " for how much that provide " situation-behavior " method below, for sake of convenience, the object that the present invention will transport with a chest conduct, and use two machine fishes to finish transport task.

1) " situation " divides

" situation " obtains according to the relative status between the problem entity, and the problem entity comprises: machine fish, chest, push away case point and impact point.Environment is divided into the chest zone, pushes away the case district, the territory is prohibited on a left side, right territory and the foreign lands of prohibiting.To push away the case zone and be to push away the case point be the center of circle, be the semicircular area of radius with 1/4 chest length, in these zones, the machine fish can directly promote chest with head.The border of prohibiting the territory is through pushing away case point, being the directed circle of radius with 1.2 times of machine fish minimal curve radius, we claim that this radius is " comfortable " radius, expression machine fish with this radius can be comfortable turning, be that the directed circle of radius is called comfortable circle with " comfortable " radius.Here we only discuss and push away case point and push away the situation that the case point is the machine fish of target with a left side corresponding to a left side.Pushing away case point and push away the case point with the right side corresponding to the right side is that the situation of the machine fish of target is similar.

Target of the present invention is to be the suitable path of machine fish planning, with guiding machine fish arrive when pushing away case point and chest in the same way, prepare for next step cooperation pushes away case.Consider that the machine fish can not stop after receiving " stopping " order at once, motion planning should make two machine fishes arrive synchronously and push away the case point.The present invention is according to the mutual relationship of problem entity, use " decision tree " to define " situation " (as shown in Figure 4), " decision tree " input be target position information, camera collection to visual information, the position that comprises machine fish and chest and directional information.Current " situation " discerned according to input information, and " decision tree " generated by following four criterions according to the binary decision rule.

Criterion 1: push away case zone criterion.This criterion is according to the machine fish and push away the relativeness in case zone, and " situation " is divided into following two classes:

The IAR situation: the machine fish is pushing away the case intra-zone.

The NIAR situation: the machine fish is pushing away the case region exterior.

Criterion 2: the feasible case direction criterion that pushes away.This criterion is divided into following two types according to the direction of machine fish and chest with the IAR situation:

The FAD situation: the machine fish is pushing away the case intra-zone, direction consistent with the chest direction (in experiment, we are unanimity with the direction difference in 45 ° of scopes), and intersect (as shown in Figure 5) with the chest border.

The NFAD situation: the machine fish is pushing away the case intra-zone, but direction and chest direction inconsistent (as shown in Figure 6).

Criterion 3: synchronous criterion.Whether this criterion also is in the FAD situation according to an other fish, and the FAD situation is divided into following two kinds of situations:

The SYN situation: another fish also is in FAD situation (as shown in Figure 5), and therefore in this case, two fishes can synchronously push away case.

The NSYN situation: another fish is not in FAD situation (as shown in Figure 7).

Criterion 4: feasible path criterion.At first we provide to give a definition:

Feasible comfortable circle: with P (φ represents direction for x, y) expression machine fish current position, and Rc represents comfortable radius, and CC represents comfortable circle, the direction of dir (x) expression x, bl, br represent that respectively a left side prohibits territory and right border of prohibiting the territory, feasible comfortable circle is defined as:

If there is a CC, (y), there is a common tangent (ptan) in x between CC and the bl (or br) through P with direction φ, and dir (ptan) is consistent with dir (CC), dir (bl) (or dir (br)), and this CC is called the feasible comfortable circle relevant with the current pose of machine fish so.

Free path: a path that is not stopped by obstacle is called free path.

Half feasible path: a path that pushes away case point to a left side from the current location of machine fish, form (as shown in Figure 8) by one section arc of feasible comfortable circle, one section arc and its directed tangent on taboo border, territory.

Feasible path: feasible path is half feasible path freely.

The NIAR situation is divided into following two kinds of situations according to feasible path:

FP situation: push away case point existence at least one feasible path to a left side from the current location of machine fish.

NFP situation: push away case point to a left side from machine fish current location and do not have feasible path (as shown in Figure 9).

The leaf node of decision tree is: SYN situation, NSYN situation, NFAD situation, FP situation, NFP situation.Because these five kinds of situations obtain by binary decision tree, so they are complete and mutual repulsions.

2) corresponding behavior design

Design corresponding to the behavior of various " situations " should be able to make the machine fish under the restriction of minimal curve radius, moves to object pose (impact point is that a left side pushes away the case point, and target direction is the direction of chest) along a suitable path.

The BSYN behavior: because success synchronously, two machine fish cooperations push away case, adjust direction simultaneously to be consistent (as shown in Figure 5) with the chest direction.

The BNSYN behavior: the NSYN situation should be avoided, in case this situation occurs, machine fish in place will stop, and waits for and another machine fish synchronous (as shown in Figure 7).

The BNFAD behavior: under the NFAD situation, though the machine fish is pushing away the case zone, direction is infeasible, therefore can not begin to push away case, and therefore, the machine fish is moved outside pushing away the case zone, up to finding a feasible path (as shown in Figure 6).

The BFP behavior: the machine fish pushes away case point along the shortest feasible path and moves (as shown in Figure 8) towards a left side.

The BNFP behavior: the machine fish pushes away case point along the shortest half feasible path and moves towards a left side, starts the barrier behavior (as shown in Figure 9) that keeps away when near barrier.

3) role assignments mechanism

In the cooperation transport task, define two roles: left comer look and right corner look.Distributed the machine fish of left comer look to require to push away case, correspondingly, distributed the machine fish of right corner look to require to push away case on the right side of chest in the left side of chest.(A, B) expression is according to the path of ordering to A from the B point of " comfortable circle " method planning with len.The present invention estimates role assignments with following cost function:

F(A)＝|WL(i)-WR(i)|+k(WL(i)+WR(j))，

WL(i)＝len(LP，P _i)+C×NL _obj，

WR(j)＝len(RP，P _j)+C×NR _obj.

Wherein, LP and RP represent that respectively a left side pushes away case point and the right side pushes away the case point; P _iAnd P _jRepresent the position that fish i and j are current respectively, if from P _iArrive LP (correspondingly, from P _jTo RP) planning the path on have obstacle, NL _Obj(correspondingly, NR _Obj) be 1; Otherwise, be 0; WL (i) and WR (j) are respectively the cost that fish i and fish j distribute left comer look and right corner look, represent the machine fish to shift to push away the time loss roughly of case point.The Task Distribution of optimizing is and minimizes F (A), and its front portion is divided into makes the arrival synchronously of two machine fishes push away the case point, and the rear section minimizes in order to make total cost.K is a positive constant, can regulate F (A) front and back two-part proportion (in experiment, k gets 0.3); C is a constant, the influence (in experiment, C gets 50) of barrier on the expression path planning.

3, based on the synchro control of fuzzy reasoning

Complete successfully collaborative task in order to be implemented under the environment that uncertain factor is many, interference is big, the present invention adopts the fuzzy control method to solve the synchro control problem.Rule-based fuzzy logic method can be used for uncertain and out of true information are carried out reasoning and decision-making.The method is by defining one group of fuzzy variable, use the notion of subordinate function to carry out reasoning from logic, finally obtaining the output of object by deblurring method.In the cooperation transport task, owing to the bump of action of inertia to object of transport, we wish to control many machine fishes can arrive object of transport synchronously for fear of the machine fish.Consider the really difficulty of control of machine milt, push away the case point in order to control the arrival synchronously of machine fish, and chest is successfully shifted to the target location that we use the method for fuzzy logic control to realize the synchro control of machine fish.

Motion planning in the synchronizing process: the present invention uses two machine fishes, pushes away the case point for two machine fishes are arrived synchronously, and we design a fuzzy controller (as shown in figure 10), shift to the speed that pushes away case point with control machine fish.Controller be input as WL (i) and WR (j), expression machine fish is shifted to the approximate time overhead of impact point; Be output as the speed of machine fish, represent the speed of left comer look machine fish and right corner look machine fish with VL and VR respectively.At first WL (i) and WR (j) be expressed as fuzzy set L, M, S}, representative is big respectively, in, little, its subordinate function (as shown in figure 11).VL and VR be expressed as fuzzy set F, M, S}, respectively representative fast, middling speed and at a slow speed, its subordinate function (as shown in figure 12).VL and VR are obtained by following fuzzy rule:

1) if WL (i) is that L and WR (j) are L, VL is F so, and VR is F;

2) if WL (i) is that L and WR (j) are M, VL is F so, and VR is M;

3) if WL (i) is that L and WR (j) are S, VL is F so, and VR is S;

4) if WL (i) is that M and WR (j) are L, VL is M so, and VR is F;

5) if WL (i) is that M and WR (j) are M, VL is F so, and VR is F;

6) if WL (i) is that M and WR (j) are S, VL is F so, and VR is S;

7) if WL (i) is that S and WR (j) are L, VL is S so, and VR is F;

8) if WL (i) is that S and WR (j) are M, VL is S so, and VR is F;

9) if WL (i) is that S and WR (j) are S, VL is M so, and VR is M.

We use the inference mode of Mamdani type, and the final speed of machine fish is obtained by " gravity model appoach " deblurring, and promptly delivery is stuck with paste the pairing speed of center of gravity that subordinate function curve and abscissa axis surround area as final output speed, and expression formula is as follows:

$\stackrel{\&OverBar;}{\mathrm{VL}}={\mathrm{\&Sigma;}}_{k=1}^9{\mathrm{VL}}_k{x}_k/{\mathrm{\&Sigma;}}_{k=1}^9{x}_k,$ $\stackrel{\&OverBar;}{\mathrm{VR}}={\mathrm{\&Sigma;}}_{k=1}^9{\mathrm{VR}}_k{x}_k/{\mathrm{\&Sigma;}}_{k=1}^9{x}_k---\left(4\right)$

x _k＝min{x _1k1，x _2k2} (5)

In the formula of (4) (5), x _kBe k bar rule " if " part the simultaneous degree (k=1 ..., 9), x _1k1(correspondingly, x _2k2) be the degree of membership of WL (i) (correspondingly, WR (j)) to k bar rule, VL _kWith VR _kBe the output that obtains from k bar rule.

4, control based on the transporting direction of fuzzy reasoning

Complete successfully collaborative task in order to be implemented under the environment that uncertain factor is many, interference is big, we adopt the fuzzy control method to solve transporting direction control problem.When two machine fishes all were in the SYN situation, ensuing task was mutual cooperation, promoted chest with head and moved to impact point.Represent the angle of chest direction with θ with respect to target direction.The direction of chest is controlled by two fish effects acting force thereon, realizes by the speed of moving about of controlling two fishes.Similarly, the speed of machine fish obtains by one group of fuzzy logic ordination.Here, fuzzy rule be input as θ, VL, VR is output.We with θ with fuzzy set PB, PM, PS, Z, NS, NM, NB} represent, and represent respectively honest, the center, just little, zero, negative little, negative in, negative big.The subordinate function of θ (as shown in figure 13).VL and VR obtain by following reasoning:

1) if θ is PB, VL is F so, and VR is S;

2) if θ is PM, VL is M so, and VR is S;

3) if θ is PS, VL is M so, and VR is S;

4) if θ is Z, VL is M so, and VR is M;

5) if θ is NS, VL is S so, and VR is M;

6) if θ is NM, VL is S so, and VR is M;

7) if θ is NB, VL is S so, and VR is F.

Similarly, VL and VR are obtained by " gravity model appoach " deblurring equally.

As shown in figure 14, the cooperate experiment porch of transportation system of the present invention is made up of decision-making level, message exchange layer and execution level, and decision-making level is made up of a main frame, and the message exchange layer is made up of sensor and transmission equipment, and execution level is made up of the machine fish.

The inventive method is example to use two machine fishes, and its concrete steps are as follows:

1, the status information of environmental information of obtaining according to camera and machine fish feedback produces control command;

2, by the message exchange layer, the control command that is produced by decision-making level sends to execution level, and the execution status of task information feedback of environmental information and machine fish is given decision-making level;

3, execution level receives and carries out the control command that sends from decision-making level.In the cooperation transport task, " comfortable " radius of two machine fishes is 33cm.

4, according to content of the present invention, the left comer look is distributed to the machine fish 1 that is positioned at the chest left side among Figure 15 (a), and the right corner look is distributed to the machine fish 2 that is positioned at the chest right side.

5, the machine fish is along moving to the target location according to the path of " comfortable circle " method planning, shown in Figure 15 (b).

6, the fuzzy logic ordination in according to the present invention, two machine fishes are moved with different speed, to realize synchronously.Shown in Figure 15 (c), in the time of 21.0 seconds, two fishes are success synchronously, and arrives and push away the case point, begins to promote object and moves towards impact point.

7, for controlling the direction of object, article two, fish is coordinated the speed of moving about by the fuzzy control method among the present invention, with the acting force (as Figure 15 (d, e) shown in) of corrective action on object, in the time of 37.0 seconds, object has successfully been moved to impact point (shown in Figure 15 (f)).

Above-mentioned only is to be explanation the present invention listed examples, various replacements, variation and the modification that on the basis that the present invention conceives substantially, can carry out, and these replacements, variation and modification should not got rid of outside protection scope of the present invention.

Claims (2)

1, a kind of cooperated transportation method of underwater bionic robot, it may further comprise the steps:

(1) based on the Pose Control of " limit cycle " principle

Be the point of contact with the impact point, be that tangential direction is made a circle of contact with the target direction, so promptly can determine center of circle B (x ₀, y ₀) the position, represent the linear velocity of robot under original coordinate system with v, represent travel direction with α, can obtain:

$v=\sqrt{{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}}^2+{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}}^2}$ ①

$\mathrm{\&alpha;}=\arctan \frac{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}}{\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}}$ ②

Wherein

$\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}=\mathrm{\&lambda;}(\stackrel{\&OverBar;}{y}-y_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{x}-x_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2))$ ③

$\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{y}}=\mathrm{\&lambda;}(-\stackrel{\&OverBar;}{x}+x_0+\mathrm{\&gamma;}(\stackrel{\&OverBar;}{y}-y_0)(r^2-{(\stackrel{\&OverBar;}{x}-x_0)}^2-{(\stackrel{\&OverBar;}{y}-y_0)}^2))$

(x, y) be robot at the planimetric coordinates of the impact point on the limit cycle under two-dimensional coordinate system, γ, λ are positive parameter, r is the radius of the circle of contact, by 1., 2., the 3. movement velocity and the direction of formula control robot, can realize the Pose Control of robot;

(2) based on the path planning of " comfortable circle " method

(a) to decision tree input target position information, camera collection to visual information, comprise the position and the directional information of robot and transportation thing;

(b) generating one group by decision tree repels and complete " situation " of having described problem space mutually;

(c) corresponding to various " situations ", the behavior of design robot makes it under the restriction of minimal curve radius, moves to object pose along a suitable path;

(d) draw the path from robot location to the target location according to " comfortable circle " rules, and distribute the role of each robot in the cooperation transportation according to the cost evaluation function;

(3) based on the synchro control of fuzzy reasoning

WL (i), WR (j) are respectively the cost that the i of robot and the j of robot are assigned as left comer look and right corner look, with WL (i), WR (j) be expressed as fuzzy set L, M, S}, representative is big respectively, in, little; With speed VL, the VR of robot be expressed as fuzzy set F, M, S}, respectively representative fast, middling speed and at a slow speed, design following fuzzy rule:

1) if WL (i) is that L and WR (j) are L, VL is F so, and VR is F;

2) if WL (i) is that L and WR (j) are M, VL is F so, and VR is M;

3) if WL (i) is that L and WR (j) are S, VL is F so, and VR is S;

4) if WL (i) is that M and WR (j) are L, VL is M so, and VR is F;

5) if WL (i) is that M and WR (j) are M, VL is F so, and VR is F;

6) if WL (i) is that M and WR (j) are S, VL is F so, and VR is S;

7) if WL (i) is that S and WR (j) are L, VL is S so, and VR is F;

8) if WL (i) is that S and WR (j) are M, VL is S so, and VR is F;

9) if WL (i) is that S and WR (j) are S, VL is M so, and VR is M, uses the inference mode of Mamdani type, and the final speed of robot is obtained by " gravity model appoach " deblurring, and expression formula is as follows:

$\stackrel{\&OverBar;}{\mathrm{VL}}={\mathrm{\&Sigma;}}_{k=1}^9{\mathrm{VL}}_k{x}_k/{\mathrm{\&Sigma;}}_{k=1}^9{x}_k,$ $\stackrel{\&OverBar;}{\mathrm{VR}}={\mathrm{\&Sigma;}}_{k=1}^9{\mathrm{VR}}_k{x}_k/{\mathrm{\&Sigma;}}_{k=1}^9{x}_k$ ④

x _k＝min{x _1k1，x _2k2} ⑤

4., 5. in the formula, x _kBe k bar rule " if " the simultaneous degree of part, k=1 wherein ..., 9; x _1k1Be the degree of membership of WL (i) to k bar rule, x _2k2Be the degree of membership of WR (j) to k bar rule, VL _kWith VR _kBe the output that obtains from k bar rule;

(4) control based on the transporting direction of fuzzy reasoning

With the chest direction with respect to the angle θ of target direction with fuzzy set PB, PM, PS, Z, NS, NM, NB} represent, and represent respectively honest, the center, just little, zero, negative little, negative in, negative big; With speed VL, the VR of robot be expressed as fuzzy set F, M, S}, respectively representative fast, middling speed and at a slow speed, design following fuzzy inference rule:

1) if θ is PB, VL is F so, and VR is S;

2) if θ is PM, VL is M so, and VR is S;

3) if θ is PS, VL is M so, and VR is S;

4) if θ is Z, VL is M so, and VR is M;

5) if θ is NS, VL is S so, and VR is M;

6) if θ is NM, VL is S so, and VR is M;

7) if θ is NB, VL is S so, and VR is F, adopts the inference mode of Mamdani type, and the final speed of robot is obtained by " gravity model appoach " deblurring equally.

2, the cooperated transportation method of a kind of underwater bionic robot as claimed in claim 1 is characterized in that: adopt following cost function F (A) to estimate role assignments:

F(A)＝|WL(i)-WR(i)|+k(WL(i)+WR(j))，

WL(i)＝len(LP，P _i)+C×NL _obj，

WR(j)＝len(RP，P _j)+C×NR _obj.

P wherein _iAnd P _jRepresent the position that i of robot and j are current respectively, LP and RP represent left impact point and right impact point respectively, len (LP, P _i) expression according to " comfortable circle " method planning from P _iThe path that point is ordered to LP, len (RP, P _j) expression according to " comfortable circle " method planning from P _jIf the path that point is ordered to RP is from P _jTo the path of LP planning, there is obstacle, NL _ObjBe 1; Otherwise be 0; If from P _jTo the path of RP planning, there is obstacle, NR _ObjBe 1; Otherwise be 0.

Images