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 method for collaborative transportation of underwater bionic robots

technical field

The invention relates to a multi-robot collaborative transport method, in particular to a collaborative transport method for underwater bionic robots.

Background technique

With the development of society, the application field of robots is constantly expanding. From automated production lines to people's daily life, from marine resource exploration to space operations, robots are everywhere. However, as far as the current level of robot technology is concerned, single robots have great limitations in terms of information acquisition, processing, control capabilities, decision-making and adaptability, etc., especially in complex tasks and dynamic environments. The ability is even more insufficient. When transporting dangerous materials, complex roads or relatively small spaces, a single robot needs to integrate various systems such as navigation system, delivery system, and communication system. The development of this highly integrated system is very difficult, the development cost is relatively high, and the development cycle is relatively long. More importantly, the single robot that integrates various systems is relatively large in size and inflexible in action. Once the single robot transport system fails, it cannot complete the task. The single-robot transport system needs to be developed for specific tasks, and each new task requires a new development of a specific transport robot, which takes a long time and is difficult to develop.

With the decrease of land resources, marine resources are getting more and more attention. The research on the exploration of marine resources and underwater reconnaissance is in full swing, and the transportation and installation of various marine detection equipment requires underwater machines to complete. However, compared with land transportation, underwater transportation has its own unique difficulties: 1. The interference in water is relatively large, so higher requirements are put forward for the anti-interference ability of the control algorithm of the underwater transportation system, and the collaborative algorithm of land robots cannot be directly applied For underwater robots, a new control method with stronger anti-interference ability needs to be proposed; 2. Due to the small underwater friction force, the braking ability of underwater robots is limited, and it is easy to overshoot. The new control method should specifically consider the braking link, which makes the design of the control method more difficult; 3. At the current stage, the movement efficiency of underwater robots is low, and energy waste is serious. New and efficient underwater robots are developed to adapt to long-term underwater operations It is imperative.

Contents of the invention

In view of the particularity and complexity of underwater robot control and the difficulty in establishing hydrodynamic models, large disturbances in water, and many uncertain factors, the purpose of this invention is to provide a new underwater robot in a complex underwater environment. Bionic robot collaborative transportation method.

In order to achieve the above object, the present invention adopts the following technical solutions: a collaborative transportation method of an underwater bionic robot, which comprises the following steps:

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

Make a tangent circle with the target point as the tangent point and the target direction as the tangent direction, then the position of the center B(x ₀ , y ₀ ) can be determined. Using v to represent the linear velocity of the robot in the original coordinate system, and α to represent the direction of motion, we can get:

$\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; =</span>\sqrt{{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&alpha;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arctan</span>}\frac{\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

in

$\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="0"\; label="y"\; filenames="C20071006475400141.png,C20071006475400191.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

$\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}<span\; class="notranslate">\; (</span><span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span>$

Both γ and λ are positive parameters, and r is the radius of the tangent circle. By controlling the movement speed and direction of the robot through (1)(2)(3), the pose control of the robot can be realized.

(2) Path planning based on the "comfort circle" method

(a) Input the target position information, the visual information collected by the camera, including the position and direction information of the robot and the transport object, etc. into the decision tree;

(b) Generate a set of mutually exclusive "cases" that fully describe the problem space via a decision tree;

(c) Corresponding to various "situations", design the behavior of the robot so that it can move towards the target pose along a suitable path under the constraint of the minimum turning radius;

(d) Plan the path from the robot position to the target position according to the "comfort circle" method, and assign the roles of each robot in the collaborative transportation according to the cost evaluation function;

(3) Synchronous control based on fuzzy reasoning

Denote WL(i), WR(j) (the cost of assigning left and right roles to robot i and robot j respectively) as a fuzzy set {L, M, S}, representing large, medium and small respectively; Velocities VL and VR are expressed as fuzzy sets {F, M, S}, representing fast, medium and slow respectively, and the following fuzzy rules are designed:

1) If WL(i) is L and WR(j) is L, then VL is F and VR is F;

2) If WL(i) is L and WR(j) is M, then VL is F and VR is M;

3) If WL(i) is L and WR(j) is S, then VL is F and VR is S;

4) If WL(i) is M and WR(j) is L, then VL is M and VR is F;

5) If WL(i) is M and WR(j) is M, then VL is F and VR is F;

6) If WL(i) is M and WR(j) is S, then VL is F and VR is S;

7) If WL(i) is S and WR(j) is L, then VL is S and VR is F;

8) If WL(i) is S and WR(j) is M, then VL is S and VR is F;

9) If WL(i) is S and WR(j) is S, then VL is M and VR is M,

Using the Mamdani-type reasoning method, the final velocity of the robot is defuzzified by the "center of gravity method", and the expression is as follows:

$\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; VL</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; VL</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; ,</span>$ $\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; VR</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; VR</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

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

In (4)(5), x _k is the simultaneous degree of the "if" part of the kth rule (k=1,...,9), x _1k1 (correspondingly, x _2k2 ) is WL(i) (correspondingly, WR(j)) is the degree of membership of the k-th rule, and VL _k and VR _k are the outputs obtained from the k-th rule.

(4) Transportation direction control based on fuzzy reasoning

The angle θ of the direction of the box relative to the direction of the target is represented by a fuzzy set {PB, PM, PS, Z, NS, NM, NB}, respectively representing positive large, positive middle, positive small, zero, negative small, negative medium, and negative large; The speed VL and VR of the robot are expressed as fuzzy sets {F, M, S}, representing fast, medium and slow speed respectively, and the following fuzzy inference rules are designed:

1) If θ is PB, then VL is F and VR is S;

2) If θ is PM, then VL is M and VR is S;

3) If θ is PS, then VL is M and VR is S;

4) If θ is Z, then VL is M and VR is M;

5) If θ is NS, then VL is S and VR is M;

6) If θ is NM, then VL is S and VR is M;

7) If θ is NB, then VL is S, VR is F,

Using the Mamdani-type reasoning method, the final velocity of the robot is also defuzzified by the "center of gravity method".

The role assignment is evaluated using the following cost function F(A):

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 .

Where P _i and P _j represent the current positions of robots i and j respectively, LP and RP represent the left target point and right target point respectively, len(LP, P _i ) (correspondingly, len(RP, P _j )) represents the The path length from P _i point (correspondingly, P _j point) to LP point (correspondingly, RP point) planned by the "comfort circle" method. NL _obj (respectively, NR _obj ) is 1 if there is an obstacle on the planned path from P _i to LP (respectively, from P _j to RP); otherwise, it is 0; WL(i) and WR(j) respectively Assign robot i and robot j costs as left and right roles.

The present invention has the following advantages due to the adoption of the above technical scheme: 1. Due to the adoption of the pose control method based on the "limit cycle" method, the speed and direction of the underwater robot can be controlled ingeniously, both in theory and in practice. It can ensure that the pose of the robot converges to the target pose, thus solving the pose control problem of the underwater robot. 2. Due to the path planning method based on the "comfort circle", the complexity of robot control in complex underwater environments is reduced, and the dimension of the problem space is simplified. In addition, the maneuverability of the underwater robot is also considered, so the path planned according to this method is feasible at any speed. 3. Due to the adoption of the motion planning method based on fuzzy reasoning, the difficulty of precise control of the underwater robot in an environment with many uncertain factors and large interference is effectively solved, and the impact of the underwater robot on the transportation object due to the inertia is prevented. , so as to achieve a smooth control of the direction of the transport object.

Description of drawings

Figure 1 is a schematic diagram of a fast-converging limit cycle

Figure 2 is a schematic diagram of a slow convergent limit cycle

Figure 3 is a schematic diagram of robot fish pose control

Figure 4 Schematic diagram of decision tree

Figure 5 is a schematic diagram of the SYN situation and its corresponding behavior obtained according to the "situation-behavior" method

Figure 6 is a schematic diagram of the NFAD situation and its corresponding behavior obtained according to the "situation-behavior" method

Figure 7 is a schematic diagram of the NSYN situation and its corresponding behavior obtained according to the "situation-behavior" method

Figure 8 is a schematic diagram of the FP situation and its corresponding behavior obtained according to the "situation-behavior" method

Figure 9 is a schematic diagram of the NFP situation and its corresponding behavior obtained according to the "situation-behavior" method

Figure 10 is a schematic diagram of the fuzzy logic controller

Figure 11 is a schematic diagram of membership functions of WL and WR in fuzzy control

Figure 12 is a schematic diagram of membership functions of VL and VR in fuzzy control

Figure 13 is a schematic diagram of the membership function of the angle between the direction of the object and the direction of the target in fuzzy control

Figure 14 is a schematic diagram of the multi-robot fish collaboration platform

Figure 15 is a schematic diagram of the collaborative transportation process of multi-robot fish

Detailed ways

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

The present invention divides the cooperative transportation problem into four sub-problems to solve: 1. pose control; 2. path planning; 3. synchronous control; 4. transportation direction control. The bionic robot fish is taken as an example below for illustration.

1. Pose control based on the "limit cycle" method

In the second-order nonlinear autonomous system, there is an important class of motion expressed by isolated closed orbits, which not only plays a very important role in the theory of differential equations itself, but also in the application of engineering technology. This kind of isolated closed orbits is called It is a "limit cycle", and any point starting from any position converges to the "limit cycle", and moves periodically along it. Since the classical control method cannot guarantee that the underwater robot reaches the target point with the correct pose, and the heuristic method cannot theoretically guarantee accurate pose control, the present invention applies the "limit cycle" theory to the robotic fish. In pose control, by skillfully controlling the speed and direction of the underwater robot, the position of the robot can converge to the limit cycle and move along the limit cycle, thus successfully solving the problem of pose control of the underwater robot.

(x₀，y₀)分别表示点A和点B在二维坐标系下的平面坐标，考虑以下非线性系统：by

(x ₀ , y ₀ ) respectively represent the plane coordinates of point A and point B in the two-dimensional coordinate system, considering the following nonlinear system:

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="0"\; label="y"\; filenames="C20071006475400141.png,C20071006475400191.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}<span\; class="notranslate">\; (</span><span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span>$

Among them, γ, λ, and r are all positive parameters. We can give the following theorem:

Theorem 1:

的运动可以用(1)式来描述，那么此系统有一个以B(x₀，y₀)为圆心、形式为 ${(\stackrel{\&OverBar;}{x}-x_0)}^2+{(\stackrel{\&OverBar;}{y}-y_0)}^2=r^2$ 的圆形“极限环”，从任意位置出发的点都收敛到此“极限环”，并绕其做周期性的运动。If in a nonlinear system, the point

The motion of can be described by formula (1), then this system has a circle with B(x ₀ , y ₀ ) as the center, the form is ${(\stackrel{\&OverBar;}{x}-x_0)}^2+{(\stackrel{\&OverBar;}{\mathrm{the\; y}}-{\mathrm{the\; y}}_0)}^2=r^2$ The circular "limit cycle" of , all points starting from any position converge to this "limit cycle", and do periodic motion around it.

向极限环的收敛速度可以通过参数γ来调节，如图1、图2所示，给出了不同参数γ下的极限环。图1是快速收敛的情况(γ＝0.001)，图2为慢速收敛的情况(γ＝0.0003)。In equation (1), the point

The convergence speed to the limit cycle can be adjusted by the parameter γ, as shown in Fig. 1 and Fig. 2, the limit cycle under different parameters γ is given. Figure 1 is the case of fast convergence (γ=0.001), and Figure 2 is the case of slow convergence (γ=0.0003).

The robot fish adopts the motion described by equation (1), its position will inevitably converge to a circular "limit cycle" and move along it, and the target pose of the robot fish can pass through the "limit cycle" when the robot fish reaches the target point. " Tangent direction to achieve. The pose control method of the robotic fish is as follows: take the target point as the tangent point, and make a tangent circle with the target direction as the tangent direction, then the position of the circle center B(x ₀ , y ₀ ) can be determined. Through the "limit cycle" method, the robot fish can achieve the specified target pose, that is, the target point is the tangent point, and the target direction is the tangent direction of the tangent circle passing through the target point (as shown in Figure 3). In order to finally realize the control of the robotic fish, the present invention converts the motion control expressed by equation (1) into the speed and direction control of the robotic fish. Using v to represent the linear velocity of the robotic fish in the original coordinate system, and α to represent the direction of motion, we can get:

$\mathrm{<span\; class="notranslate">\; v</span>}<span\; class="notranslate">\; =</span>\sqrt{{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; \&lambda;</span>}\sqrt{{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; \&alpha;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arctan</span>}\frac{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}}{\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}}<span\; class="notranslate">\; =</span>\frac{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}<span\; class="notranslate">\; .</span>$

In (2), (3) formula, $\tilde{x}=\stackrel{\&OverBar;}{\mathrm{the\; y}}-{\mathrm{the\; <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;}{\mathrm{the\; y}}-{\mathrm{the\; y}}_0)}^2),$

$\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; r</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{<span\; class="notranslate">\; (</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

It can be seen from formula (2) that by adjusting the value of the parameter λ, the size of v can be adjusted arbitrarily. The pose control of the robot fish can be realized by adopting the motion method expressed in (2) and (3).

2. Path planning based on the "comfort circle" method

In order to reduce the complexity of underwater robot control and simplify the dimension of the problem space as much as possible, the present invention adopts a novel "situation-behavior" method based on "comfort circle" to solve the path planning problem.

Considering the difficulty of precise motion control of robotic fish, the present invention uses a "situation-behavior" design method to reduce the complexity of path planning in cooperative transportation. The "situation-action" approach is a design approach based on defining a set of "situations" that describe the relative states among problem entities. This design method satisfies the following conditions: a) the "situations" are mutually exclusive and completely represent the relative state between the problem entities; and the defined "situation" space cannot be too large; b) each "situation" The corresponding behavior can independently solve the related problem. The advantage of using the "situation-behavior" method is that it is a "segmentation" strategy, thus reducing the complexity of the task; in addition, there is no need to consider the problem of behavior fusion, because the "situation" constitutes a complete state space , and are mutually exclusive. The geometric realization of the "situation-behavior" method - "comfort circle method" is given below. For the convenience of description, the present invention uses a box as the object to be transported, and uses two robot fish to complete the transport task.

1) "Situation" division

"Situation" is obtained according to the relative state among problem entities, which include: robotic fish, box, push box point and target point. The environment is divided into box area, push box area, left forbidden area, right forbidden area and outer area. The push box area is a semicircular area with the box push point as the center and 1/4 of the box length as the radius. In these areas, the robot fish can directly push the box with its head. The boundary of the forbidden area is a directed circle with a radius of 1.2 times the minimum turning radius of the robotic fish passing through the push box point. We call this radius "comfortable" radius, which means that the robotic fish can turn comfortably with this radius, and the radius is "comfortable". A directed circle whose radius is radius is called a comfort circle. Here we only discuss the case corresponding to the left push box point and the robotic fish targeting the left push box point. The situation is similar for the robotic fish corresponding to the right push box point and targeting the right push box point.

The object of the present invention is to plan an appropriate path for the robotic fish, so as to guide the robotic fish to the same direction as the box when it arrives at the box-pushing point, so as to prepare for the next step of collaborative box-pushing. Considering that the robotic fish cannot stop immediately after receiving the "stop" command, the motion planning should make the two robotic fish arrive at the pushing point synchronously. According to the relationship between problem entities, the present invention uses "decision tree" to define "situation" (as shown in Figure 4), and the input of "decision tree" is target position information, visual information collected by camera, including robotic fish and box location and orientation information. The current "situation" is identified according to the input information, and the "decision tree" is generated by the following four criteria according to the binary decision rule.

Criterion 1: Criterion for push box area. According to the relative relationship between the robotic fish and the pushing box area, this criterion divides the "situations" into the following two categories:

IAR situation: The robotic fish is inside the pushing box area.

NIAR Situation: The robotic fish is outside the pushing box area.

Criterion 2: Criterion for feasible push box direction. This criterion divides the IAR situation into the following two types according to the direction of the robot fish and the box:

FAD situation: the robotic fish is inside the pushing box area, the direction is consistent with the direction of the box (in the experiment, we take the direction difference as consistent within 45°), and intersects with the box boundary (as shown in Figure 5).

NFAD situation: the robotic fish is inside the pushing box area, but the direction is not consistent with the direction of the box (as shown in Figure 6).

Criterion 3: Synchronization criterion. This criterion divides the FAD situation into the following two situations according to whether another fish is also in the FAD situation:

SYN situation: The other fish is also in the FAD situation (as shown in Figure 5), so in this case both fish can push the box synchronously.

NSYN condition: The other fish is not in FAD condition (as shown in Figure 7).

Criterion 4: Feasible path criterion. First we give the following definitions:

Feasible comfort circle: P(x, y) represents the current position of the robotic fish, φ represents the direction, Rc represents the comfort radius, CC represents the comfort circle, dir(x) represents the direction of x, bl, br represent the left forbidden area and The boundary of the right forbidden area, the feasible comfort circle is defined as:

If there is a CC, passing through P(x, y) in the direction φ, there is a common tangent (ptan) between CC and bl (or br), and dir(ptan) and dir(CC), dir(bl) (or dir(br)) are consistent, then this CC is called a feasible comfort circle related to the current pose of the robotic fish.

Free Path: A path that is not blocked by obstacles is called a free path.

Semi-feasible path: a path from the current position of the robotic fish to the left push box point, consisting of an arc of the feasible comfort circle, an arc of the forbidden area boundary and its directional tangent (as shown in Figure 8).

Feasible path: A feasible path is a free semi-feasible path.

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

FP case: There is at least one feasible path from the current position of the robotic fish to the left push box point.

NFP situation: There is no feasible path from the current position of the robotic fish to the left push box point (as shown in Figure 9).

The leaf nodes of the decision tree are: SYN case, NSYN case, NFAD case, FP case, NFP case. Since these five cases are obtained through binary decision trees, they are complete and mutually exclusive.

2) Corresponding behavior design

Behavior design corresponding to various "situations" should enable the robotic fish to move along a suitable path to the target pose (the target point is the left push box point, and the target direction is the direction of the box) within the limit of the minimum turning radius .

BSYN behavior: Since the synchronization has been successful, the two robotic fish cooperate to push the box and adjust the direction to keep the same direction as the box (as shown in Figure 5).

BNSYN Behavior: NSYN situation should be avoided, once this happens, the robotic fish already in place will stop and wait for synchronization with another robotic fish (as shown in Figure 7).

BNFAD behavior: In the case of NFAD, although the robot fish is already in the push box area, but the direction is not feasible, so it cannot start to push the box. Therefore, the robot fish moves outside the box push area until it finds a feasible path (as shown in Figure 6 ).

BFP behavior: the robotic fish moves toward the left push box point along the shortest feasible path (as shown in Figure 8).

BNFP behavior: the robotic fish moves toward the left push box point along the shortest semi-feasible path, and starts obstacle avoidance behavior when approaching an obstacle (as shown in Figure 9).

3) Role assignment mechanism

In the collaborative transport task, two roles are defined: left role and right role. The robot fish assigned the left role asked to push the box on the left side of the box, and correspondingly, the robot fish assigned the right role asked to push the box on the right side of the box. Let len(A, B) represent the path length from point B to point A planned according to the "comfort circle" method. The present invention uses the following cost function to evaluate 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 .

Among them, LP and RP represent the left push box point and the right push box point respectively; P _i and P _j represent the current positions of fish i and j respectively, if the plan from P _i to LP (correspondingly, from P _j to RP) There is an obstacle on the path, NL _obj (correspondingly, NR _obj ) is 1; otherwise, it is 0; WL(i) and WR(j) assign the cost of the left role and the right role to fish i and fish j respectively, representing the robotic fish Approximate time spent moving to the push box point. The optimal task assignment is to minimize F(A), the first part is to make the two robotic fish arrive at the pushing point synchronously, and the second part is to minimize the total cost. k is a normal number, which can adjust the proportion of the two parts before and after F(A) (in the experiment, k is 0.3); C is a constant, indicating the influence of obstacles on the planned path (in the experiment, C is 50).

3. Synchronous control based on fuzzy reasoning

In order to successfully complete the cooperative task in an environment with many uncertain factors and great interference, the present invention adopts a fuzzy control method to solve the synchronous control problem. The rule-based fuzzy logic method can be used to reason and make decisions on uncertain and imprecise information. This method is to define a group of fuzzy variables, use the concept of membership function to carry out logical reasoning, and finally obtain the output of the object through the defuzzification method. In the cooperative transportation task, in order to avoid the impact of the robot fish on the transportation object due to the inertia, we hope to control multiple robot fish to reach the transportation object synchronously. Considering the difficulty of precise control of the robotic fish, in order to control the robotic fish to reach the pushing point synchronously and successfully move the box to the target position, we use the method of fuzzy logic control to realize the synchronous control of the robotic fish.

Motion planning in the synchronization process: the present invention uses two robotic fishes. In order to make the two robotic fishes arrive at the push box point synchronously, we design a fuzzy controller (as shown in Figure 10) to control the robotic fish to move to the push box point speed. The input of the controller is WL(i) and WR(j), which represent the approximate time cost for the robot fish to move to the target point; the output is the speed of the robot fish, and VL and VR represent the speed of the left and right role robot fish respectively. speed. First, WL(i) and WR(j) are represented as fuzzy sets {L, M, S}, which represent large, medium and small respectively, and their membership functions (as shown in Figure 11). VL and VR are expressed as fuzzy sets {F, M, S}, representing fast, medium and slow respectively, and their membership functions (as shown in Figure 12). VL and VR are obtained by the following fuzzy rules:

1) If WL(i) is L and WR(j) is L, then VL is F and VR is F;

2) If WL(i) is L and WR(j) is M, then VL is F and VR is M;

3) If WL(i) is L and WR(j) is S, then VL is F and VR is S;

4) If WL(i) is M and WR(j) is L, then VL is M and VR is F;

5) If WL(i) is M and WR(j) is M, then VL is F and VR is F;

6) If WL(i) is M and WR(j) is S, then VL is F and VR is S;

7) If WL(i) is S and WR(j) is L, then VL is S and VR is F;

8) If WL(i) is S and WR(j) is M, then VL is S and VR is F;

9) If WL(i) is S and WR(j) is S, then VL is M and VR is M.

We use the Mamdani-type reasoning method. The final speed of the robotic fish is defuzzified by the "center of gravity method", that is, the speed corresponding to the center of gravity of the area enclosed by the fuzzy membership function curve and the abscissa axis is taken as the final output speed. The expression is as follows :

$\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; VL</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; VL</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; ,</span>$ $\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; VR</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; VR</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 9</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

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

In (4)(5), x _k is the simultaneous degree of the "if" part of the kth rule (k=1,...,9), x _1k1 (correspondingly, x _2k2 ) is WL(i)( Correspondingly, WR(j)) is the membership degree of the k-th rule, and VL _k and VR _k are the outputs obtained from the k-th rule.

4. Transportation direction control based on fuzzy reasoning

In order to achieve successful completion of collaborative tasks in an environment with many uncertain factors and large disturbances, we use the fuzzy control method to solve the problem of transportation direction control. When the two robot fish are in the SYN situation, the next task is to cooperate with each other and push the box with the head to move to the target point. Let θ denote the angle of the box orientation with respect to the target orientation. The orientation of the box is controlled by the forces exerted by the two fish on it, by controlling the swimming speed of the two fish. Similarly, the speed of the robotic fish is obtained by a set of fuzzy logic rules. Here, the input of the fuzzy rule is θ, VL, VR is the output. We represent θ with a fuzzy set {PB, PM, PS, Z, NS, NM, NB}, representing positive big, positive middle, positive small, zero, negative small, negative middle, and negative big. The membership function of θ (as shown in Figure 13). VL and VR are obtained by the following reasoning:

1) If θ is PB, then VL is F and VR is S;

2) If θ is PM, then VL is M and VR is S;

3) If θ is PS, then VL is M and VR is S;

4) If θ is Z, then VL is M and VR is M;

5) If θ is NS, then VL is S and VR is M;

6) If θ is NM, then VL is S and VR is M;

7) If θ is NB, then VL is S and VR is F.

Similarly, VL and VR are also deblurred by the "centroid method".

As shown in Figure 14, the experimental platform of the collaborative transportation system of the present invention is composed of a decision-making layer, an information exchange layer and an execution layer. The decision-making layer is composed of a host computer, the information exchange layer is composed of sensors and transmission equipment, and the execution layer is composed of robotic fish. .

The method of the present invention takes the use of two robot fish as an example, and its specific steps are as follows:

1. Generate control commands according to the environmental information obtained by the camera and the state information fed back by the robotic fish;

2. Through the information exchange layer, the control commands generated by the decision-making layer are sent to the execution layer, and the environmental information and the task execution status information of the robotic fish are fed back to the decision-making layer;

3. The executive layer receives and executes the control commands sent from the decision-making layer. In the collaborative transport task, the "comfort" radius of the two robotic fish was 33cm.

4. According to the content of the present invention, the left role is assigned to the robotic fish 1 on the left side of the box in Figure 15(a), and the right role is assigned to the robotic fish 2 on the right side of the box.

5. The robot fish moves to the target position along the path planned according to the "comfort circle" method, as shown in Figure 15(b).

6. According to the fuzzy logic rules in the present invention, the two robot fish move at different speeds to achieve synchronization. As shown in Figure 15(c), at 21.0 seconds, the two fish have been synchronized successfully, and have reached the push box point, and began to push the object towards the target point.

7. In order to control the direction of the object, the two fish coordinate the swimming speed through the fuzzy control method in the present invention to adjust the active force acting on the object (as shown in Figure 15 (d, e)), at 37.0 seconds , the object has been successfully moved to the target point (as shown in Figure 15(f)).

The above are only examples enumerated to illustrate the present invention, and various replacements, changes and modifications can be made on the basis of the basic concept of the present invention, and these replacements, changes and modifications should not be excluded from the 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