CN1595312A - Motion control method for multi-joint bionic machine fish - Google Patents

Abstract

It is multiple-joint bionic machine fish move control method, which comprises the following: to control the speed of the machine fish straight moving through adjusting the swing frequency of the joints; to control the machine fish direction through exerting different joint excursion. This invention provides the theory design of the multiple-joint machine fish with preference and realizes the moving flexible control and can be applied in design of underwater aircraft, underwater exploration, underwater saving, underwater device maintenance and inspect and defense of the sea domain.

Description

The motion control method of multi-joint bionic machine fish

Technical field

The present invention relates to the motion control method of multi-joint bionic machine fish.

Background technology

As the binding site of biomechanism and engineering, bionic machine fish has become a multidisciplinary research that comprises hydrodynamics control and Driving technique.Because the motion of bionic machine fish in water relates to the hydrodynamics of fluid environment and the kinematics of fish body, on the basis of existing Hydrodynamics Analysis, also can't set up the complicated Hydrodynamic Model of machine fish swimming, therefore be difficult to set up design and the control that an accurate hydrodynamics propulsion model comes the guidance machine fish by the method for resolving.The fish anthropologist studies show that, is implying one in the ahead running of fish by the capable ripple of back neck to the afterbody propagation.Inspired by this, the propelling that people's trial comes researching fish from kinematic angle is to avoid complicated Hydrodynamics Analysis.1996, seven of efficient that the people such as Barrett of U.S. MIT have proposed the propulsion model of a tuna (typical Scad section fish) and the decision tuna of having derived moves about major parameters independently.How rationally choosing the fish of these 7 independent parameter for variety classes, profile and size, still is a very stubborn problem.When adopting the way of experiment one by one, suppose corresponding 10 values of each parameter range, the element number of lab space is up to 7 ¹⁰=282475249.So based on the kinematics model that simple normality is moved about, studying a kind of number of parameters machine fish swimming control method few, that be convenient to control, be easy to realize is the guardian technique problem of machine fish Design Theory and motion control.

Summary of the invention

The purpose of this invention is to provide the motion control method of a kind of machine fish that advances based on the imitative Scad section pattern of articulation structure, avoided complicated Hydrodynamics Analysis, can realize more accurate motion control.

For achieving the above object, a kind of motion control method of multi-joint bionic machine fish comprises step:

Control the speed that the machine fish is kept straight on and moves about by the hunting frequency of regulating the joint;

By applying the direction of different joint skew control machine fishes.

The present invention will provide for the Design Theory of multi-joint bionic machine fish with reference to and realize motor-driven, the control of moving about flexibly, can be applied to the design of submarine navigation device, under water resource exploration, under water salvage, underwater facility maintenance and patrol and examine and the aspects such as defence of maritime territory.

Description of drawings

Fig. 1 is based on the fish bulk wave curve fitting figure of articulation structure;

Fig. 2 is the mechanical construction drawing of machine fish.

Embodiment

Implying a capable ripple of being propagated to afterbody by the back neck in the ahead running of fish, this translatory wave mainly shows the bending of backbone and musculature, its amplitude by forward direction after increase gradually, its velocity of propagation is greater than the pace of fish body.The fish bulk wave curve of Scad section fish can be regarded fish bulk wave width of cloth envelope and sinusoidal synthetic as, and it starts from the center of the inertial force of fish body, extends to caudal peduncle, and its curvilinear equation can be expressed as:

y _body(x，t)＝[(c ₁x+c ₂x ²)][sin(kx+ωt)]????(1)

Here, y _BodyBe the transversal displacement of fish body, x is the axial displacement of fish body, and k is that (k=2 π/λ), λ is the wavelength of fish bulk wave to the bulk wave number, c ₁Be an once coefficient of fish bulk wave wave amplitude envelope, c ₂Be the quadratic term coefficient of fish bulk wave wave amplitude envelope, ω is fish bulk wave frequency (ω=2 π f=2 π/T).

This fluctuation principle of propeller according to Scad section fish, can design multiarticulate bionic machine fish by the fish bulk wave curve in match (1) formula, and the present invention is primarily aimed at the control problem of moving about of coming the curvilinear motion of match fish bulk wave, imitative Scad section pattern machine fish with the individual joint of N (N 〉=2).The joint of bionic machine fish drives by motor direct-drive or by actuated elements such as wire rope, pull bars.As its data of moving about, can realize the move about control of machine fish in water with two dimension swing array provided by the present invention.

Because computer control is the control of a kind of digital quantity, we consider fish bulk wave curve discretize in the present invention, time parameter t from fish body-wave motion function y _Body(x separates in t).That is to say, the fish bulk wave of constantly propagating is resolved into two parts: 1) in hunting period, with irrelevant SPL sequence (seeing formula (2)) y of time _Body(x, i) (i=0,1 ..., M-1); 2) with the hunting frequency f of time correlation, i.e. the number of times of tilting mechanism perfect aspect wave motion in the unit interval.

$y_{\mathrm{body}}(x,i)=\left[(c_{1}x+c_2{x}^2)\right]\left[\sin (\mathrm{kx}-\frac{2\mathrm{\π}}{M}i)\right]----\left(2\right)$

Here, i is the SPL sequence variable in the hunting period; M is a fish bulk wave resolution, and promptly whole fish bulk wave is limited to the highest hunting frequency of servomotor by discrete degree on it in a hunting period.

Before match fish bulk wave curve, we define fish bulk wave relative wavelength R (0＜R≤1) accounts for whole sine wave for the length of fish body swing part ratio.When R near 0 the time, fish body swing part can be considered a rigid rod, is difficult to produce propelling power; When R near 1 the time, fish body swing part presents a complete sine wave; When R obtained suitable value, the motion of fish body in water relatively steadily, efficiently.According to the experimental knowledge that theoretical analysis and experiment are obtained, the present invention mainly is that DC servo motor drives to adopting, the robot fish movement of fish body relative wavelength about 0.3-0.6 uses wobble data provided by the invention when controlling.

As shown in Figure 1, the fish body is represented at the linkage assembly of the motion (between the axial area of 0 to R π 2) of swinging plane available a series of (N) series connection.The length of supposing every bar is respectively l ₁, l ₂..., l _N(l here, _iLength the unknown, but l ₁, l ₂..., l _NBetween the length ratio can set in advance, might as well establish: l ₁: l ₂: ...: and l _N=l ₁': l ₂': ...: l _N'), its corresponding joint angle is φ ₁, φ ₂..., φ _N, its corresponding end points coordinate is (x ₀, y ₀), (x ₁, y ₁) ..., (x _N, y _N), the control of machine fish maneuverability mainly relies on inner shape (joint angle φ _j, the change of 1≤j≤N), the control of its speed relies on the change of hunting frequency f.In case selected fish bulk wave wave amplitude envelope coefficient (c ₁, c ₂) and bulk wave count k (k=1), the fish bulk wave shape in a certain moment has just been determined substantially.

On mathematical meaning, for the different joint angle φ of (i) constantly in the hunting period _IjCan calculate by match fish bulk wave curve.Problem to be solved is to seek suitable joint angle φ 0 between the axial area of R π 2 _IjMake l _jHorizontal ordinate end to end and last root bar distal point just drops on R π 2 places on fish bulk wave curve.Promptly satisfy following condition (seeing formula (3)):

In the formula, (x _{I, j}, y _{I, j}) be the end points coordinate of i moment j root bar in the hunting period, x _{I, 0}=0, x _{I, N}=R * 2 π, 1≤j≤N, 0≤i≤M-1.

For system of equations (3), owing to comprise x _{I, j}Quadratic term, and l _jThe unknown when N increases, is difficult to the exact solution (x by the method group of equations of resolving _{I, j}, y _{I, j}).And always introducing round-off error in the actual operation, also can only be approximate solution even adopt separating that accurate method tries to achieve.In this case, the present invention adopts iterative algorithm, and it with same calculation procedure, repeated calculation, tries to achieve the approximate solution of system of equations by choosing certain initial value.Its concrete steps are as follows:

(a) the long Coefficient m of iterative approach bar

To equation: (x _{I, j}-x _{I, j-1}) ²+ (y _{I, j}-y _{I, j-1}) ²=l _j ²Transplant the instrument error function: f (x _{I, j-1}, y _{I, j-1}, x _{I, j}, y _{I, j}, m)=(x _{I, j}-x _{I, j-1}) ²+ (y _{I, j}-y _{I, j-1}) ²-(m * l _j') ²(m is the long coefficient of bar here), the initial value (m=0.0) of selection m begins iterative computation from j=1, i=0, up to satisfying f (x _{I, j-1}, y _{I, j-1}, x _{I, j}, y _{I, j}, m)＜L _Error(L _ErrorBe the long error of setting of bar), follow j=j+1, change the iterative computation of next root bar over to, up to calculating N root bar, differentiate x _NWhether equal R * 2 π, promptly calculate | x _N-R * 2 π |＜X _ErrorWhether set up.If this absolute error expression formula is set up, this moment, the m value was the long coefficient of the bar of being asked, loop ends.Otherwise m=m+STEP (STEP is an iteration step length, gets STEP=0.01 here) changes next time circulation over to, carries out the iterative computation from the 1st bar to N root bar, up to satisfying the loop ends condition | and x _N-R * 2 π |＜X _Error

(b) calculate joint coordinates to (x _Ij, y _Ij)

The long Coefficient m of bar during the record loop ends is because l _j=m * l _j' with l _jApproximate value back substitution system of equations (3), the group of solving an equation can be obtained coordinate successively to (x ₀₁, y ₀₁), (x ₀₂, y ₀₂) ..., (x _0N, y _0N).I=i+1, the coordinate that changes next moment (i+1) over to is to (x ₁₁, y ₁₁), (x ₁₂, y ₁₂) ..., (x _1N, y _1N) find the solution.Repeated calculation is up to calculating i=M-1 coordinate constantly to (x _M-1,1, y _M-1,1), (x _M-1,2, y _M-1,2) ..., (x _{M-1, N}, y _{M-1, N}).

(c) the angle γ of calculating bar (lj) and fish body main shaft (x axle) _Ij

End points coordinate (x according to any time i, j root bar in the hunting period of obtaining in the previous step _Ij, y _Ij), utilize formula (4) can obtain this bar (l _j) with the angle γ of fish body main shaft (x axle) _Ij(seeing formula (4)):

γ _i，j＝a?tan?2((x _i，j-x _i，j-1)，(y _i，j- _yi，j-1))×180°/π??????(4)

(d) calculate two-dimentional joint swing array OscData[M] [N]

Utilize the bar that previous step obtains and the angle γ of the fish body longitudinal axis _Ij, can obtain the included angle of any time i in hunting period, j-1 root bar and j root bar _Ij(seeing formula (5)):

φ _i，j＝γ _ij-γ _i，j-1??????????????????????(5)

Here, suppose γ _{I, 0}=0 °.

Thereby array OscData[M is swung in the two-dimentional joint that can obtain a M * N] [N] (seeing formula (6)):

$\mathrm{OscData}\left[M\right]\left[N\right]=\left[\begin{array}{cccc}{\mathrm{\φ}}_{01}& {\mathrm{\φ}}_{02}& \·\·\·& {\mathrm{\φ}}_{0N}\\ {\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& \·\·\·& {\mathrm{\φ}}_{1N}\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ {\mathrm{\φ}}_{M-\mathrm{1,1}}& {\mathrm{\φ}}_{M-\mathrm{1,2}}& \·\·\·& {\mathrm{\φ}}_{M-1,N}\end{array}\right]----\left(6\right)$

The move about data of array as the machine fish are swung in the two-dimentional joint that utilizes formula (6) to draw, and by applying different joint skews, can adjust the distorted shape of machine fish swing part, thereby realize different direction control.Simultaneously, in the process of moving about, can change the propelling power of machine fish in water by changing hunting frequency f, and then realize different speed control.Because these motion control parameters are all irrelevant with the profile and the size of fish body, so only rely on parameter set { φ ₁, φ ₂..., φ _N, f} just can realize the control of machine fish in water.

When needing direction controlled in machine fish swimming process, adopt the method that the wobble data in 1 to joint K K the joint connected in series from the joint is increased the fixed deflection angle, wherein number K in joint is less than N/2.When moving under the wobble data control of machine fish in (6) formula, in the time of need turning to the direction of α degree as if the machine fish, the wobble data of (6) formula will be adjusted into

$\mathrm{OscData}\left[M\right]\left[N\right]=\left[\begin{array}{cccc}{\mathrm{\φ}}_{01}+\mathrm{\α}& {\mathrm{\φ}}_{02}+\mathrm{\α}& \·\·\·& {\mathrm{\φ}}_{0N}\\ {\mathrm{\φ}}_{11}+\mathrm{\α}& {\mathrm{\φ}}_{12}+\mathrm{\α}& \·\·\·& {\mathrm{\φ}}_{1N}\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ {\mathrm{\φ}}_{M-\mathrm{1,1}}+\mathrm{\α}& {\mathrm{\φ}}_{M-\mathrm{1,2}}+\mathrm{\α}& \·\·\·& {\mathrm{\φ}}_{M-1,N}\end{array}\right]----\left(7\right)$

The joint of wherein adding α degree deviation angle is Φ ₁-Φ _N/2Between from Φ ₁Continuous K joint of beginning, the K value can be definite according to real needs, but must not be greater than N/2.Direction deflection angle α is a benchmark with the center line of fish body front portion, and it changes counterclockwise for timing machine fish | α |, the machine fish changeed clockwise when it was negative | α | and, the maximal value of α is subjected to the designed machine fish mechanism and the characteristic limitations of drive motor.When α is 0 when spending, (7) are reduced to (6) formula.The machine fish adopts (7) just can realize divertical motion in water when formula is controlled.

Embodiment

The four joint machine fishes that the motion control method of multi-joint bionic machine fish provided by the invention is applied to Institute of Automation Research of CAS's complication system and intelligence science laboratory development see Fig. 2.The joint of this machine fish adopts DC servo motor to drive, and concrete technical parameter sees Table 1.

The technical parameter of table 1 machine fish

Size (length * wide * height)	～400×40×78mm 3
		Weight	～0.5kg
Close joint number	4
		The full swing frequency	2Hz (in the water)
Swing part length	178mm
		Maximum trip speed (craspedodrome)	～0.32m/s
Minimal curve radius	～200mm
		Peak torque	3.2Kg×cm
Operating voltage	4.8V
		Drive pattern	DC servo motor
Communication mode	Radio frequency (315M)

$\mathrm{OscData}\left[18\right]\left[4\right]=\left[\begin{array}{cccc}4.07& -3.28& 6.85& 12.68\\ 2.46& -5.84& 3.65& 12.64\\ 0.55& -7.73& 0.06& 10.87\\ -1.43& -8.70& -3.47& 7.66\\ -3.25& -8.63& -6.57& 3.59\\ -4.66& -7.50& -8.94& -0.744\\ -5.51& -5.44& -10.30& -4.85\\ -5.69& -2.71& -10.41& -8.43\\ -5.20& 0.31& -9.23& -11.16\\ -4.07& 3.28& -6.85& -12.68\\ -2.46& 5.85& -3.65& -12.64\\ -0.55& 7.73& -0.06& -10.87\\ 1.43& 8.70& 3.47& -7.66\\ 3.25& 8.63& 6.57& -3.59\\ 4.66& 7.50& 8.94& 0.74\\ 5.51& 5.44& 10.29& 4.85\\ 5.69& 2.71& 10.42& 8.43\\ 5.20& -0.31& 9.23& 11.16\end{array}\right]----\left(8\right)$

The swing control data of machine fish is integrated in the micro controller of head.By micro controller control CPLD, CPLD produces pwm signal through modulation, controls the corner of each DC servo motor, thereby realizes the suitable swing in each joint.In the hunting period of machine fish, the parameter of fish bulk wave match is as follows: c ₁=0.1, c ₂=0, k=1, R=0.5, M=18, N=4, l ₁': l ₂': l ₃': l ₄'=1: 1: 1: 1.The final result of this propulsion model is one 18 * 4 joint swing array: OscData[18] [4] (seeing formula (8), unit: spend) and hunting frequency f.

In the speed control experiment of machine fish, by changing the hunting frequency f of servomotor, the craspedodrome that obtains the machine fish relation of speed and frequency (seeing Table 2) of moving about: the speed of moving about is along with the increase face of hunting frequency increases, but the maximum speed of moving about is limited by the full swing frequency of servomotor.

The move about relation of speed (m/s) of the hunting frequency (Hz) of the motor of table 2 machine fish and craspedodrome

Frequency (Hz)	??0.5	??0.57	??0.67	??0.8	??1.0	??1.34	??2.0
								Speed (m/s)	??0.12	??0.16	??0.18	??0.20	??0.23	??0.26	??0.32

In the direction control experiment of machine fish, test allows the machine fish finish the circular motion of one 2 π with different radiuss of turn.Here, suppose all joint angle φ _iThe centre position (being parallel to axon) of (i=1,2,3,4) is 0 °, and the anticlockwise limit is-30 °, and the right rotation limit is 30 °.If the whole slewing area of motor is divided into 8 grades (4 ,-3 ,-2 ,-1 grade is left-hand rotation, and 1,2,3,4 grades for turning right), represents 7.5 ° for every grade.When selecting different turning shelves (L), in each hunting period, corresponding joint angle side-play amount (L * 7.5 °) is superimposed to preceding two joint angle (φ ₁And φ ₂), thereby the fish body bends towards a side, realizes the turning of different radii.Relevant experimental data sees Table 3.

The angular velocity (rad/s) of table 3 machine fish when hunting frequency is 2Hz

Class	-4	-3	-2	-1	???0	???1	???2	???3	???4
										Angular velocity	-1.2	-0.92	-0.8	-0.5	???-0.1～0.1	???.3	???.45	???.95	???1.1

Claims (5)

1. the motion control method of a multi-joint bionic machine fish comprises step:

Control the speed that the machine fish is kept straight on and moves about by the hunting frequency of regulating the joint;

By applying the direction of different joint skew control machine fishes.

2. by the described method of claim 1, it is characterized in that described machine fish swing comprises step:

The long Coefficient m of iterative approach bar;

Calculate joint coordinates to (x _Ij, y _Ij);

Calculate the angle γ of bar (li) and fish body main shaft (x axle) _Ij

Calculate two-dimentional joint swing array OscData[M] [N].

3. by the described method of claim 1, the joint that it is characterized in that described machine fish is a plurality of joints that are connected in series.

4. by the described method of claim 1, it is characterized in that described direction control comprises step:

Wobble data to 1 to joint K K the joint connected in series from the joint increases the fixed deflection angle, and wherein number K in joint is less than N/2.

5. by the described method of claim 1, control separates with direction to it is characterized in that described speed control.

Images