CN105629966A - Geometric design method for multilayer encircling formation enclosure - Google Patents

Abstract

The invention discloses a geometric design method for a multilayer encircling formation enclosure. A target hyper-ellipsoidal surface in the multilayer encircling formation enclosure, an expected simple convex closed rail on the target hyper-ellipsoidal surface and the dynamic state of a moving object are described in a hyperquadric central coordinate system. The target hyper-ellipsoidal surface is expanded in a mode of concentric compression as an equivalent hyper-ellipsoidal surface cluster relative to a curved surface function. The movable range of an i-th moving object is determined through the regularity of a curved surface. The method is especially suitable for the description of a hyper-ellipsoid and a rail and dynamic state of the moving object in the hyperquadric central coordinate system. The method is simple and reliable, is higher in precision, and can be used for coordinative collection.

Description

The geometric design method that the formation of a kind of Multi-layered wound is surrounded

Technical field

The present invention relates to the geometric design method that the formation of a kind of Multi-layered wound is surrounded.

Background technology

The movable body composition mobile sensor network utilizing limited installation sensor carries out collaborative target information collection, because the robustness of its brilliance and extensibility are by the concern by lot of domestic and foreign scientific research institution and famous scholar. NASA as far back as last century the nineties just set about carrying out Marsokhod cooperative exploring Mars program (NASA, " RoboticMarsExploration ", http://www.nasa.gov/mission_pages/mars-pathfinder/index.html), nearly 20 days so far. The beginning of this century, Princeton University combines 12 research institutions and has carried out the experiment (PrincetonUniversity, " AdaptiveSamplingandPrediction " .http: //www.princeton.edu/dcsl/asap/) of underwater robot Collect jointly marine organisms community information. Celestial body in order to comply with future explores task, and China has also progressively carried out Lunar Probe Project. When performing collaborative target information acquisition tasks, in order to utilize limited movable body (movable sensor) gather data to greatest extent and ensure to gather the precision of data fully, typically require the track planning each movable body and require that multiple movement bodies forms certain formation on given track, namely surrounding control technology around forming into columns.

Currently, (Chen Yangyang, Tian Yuping, based on the multirobot trailing formation control design case method of track extension, the patent No.: ZL201010552508.4 to be concentrated mainly on two-dimensional space around encirclement control technology of forming into columns; Chen Yangyang, Tian Yuping, the trailing formation control method of multiple movement bodies in three dimensions, the patent No.: ZL200910184547.0). But, for the collection information of the shoal of fish, micropopulation and Field of Salt distribution etc. in deep-sea, it is necessary to multiple movement bodies multilayer stereo carries out information gathering round target group round and round; Space celestial body detecting then needs multiple movement bodies to form into columns around tested star along different curved surfaces as surrounding star; Consider that inspected object complexity is (such as tested celestial body various shapes, tested field distribution variation), this is accomplished by designed controller and not only wants to realize logging in of curved surface (particularly super ellipsoids face), the expectation track that movable body moves on target surface can also be ordered about, ensure the formation relation between multiple movement bodies simultaneously, here it is our described Multi-layered wound formation control problem, it is clear that existing method cannot solve problems. It is furthermore noted that existing needs alternately for two-way/oriented information around formation control method adopts different design of control laws. But, the communication equipment in reality is because being subject to external interference, and two-way communication can become oriented communication often, and end of interrupt communication reverts to again two-way communication. Topological alternately for oriented/two-way information, if adopting different surrounding to form into columns surround controller, this undoubtedly can to realizing bringing a lot of troubles.

Therefore, it is adaptable to the Multi-layered wound formation of oriented/two-way information interaction topology is surrounded the method for designing controlled and will more be had realistic meaning. But there is not yet this type of control method at present.

Summary of the invention

Goal of the invention: in order to overcome the deficiencies in the prior art, the present invention provides the geometric design method that a kind of Multi-layered wound forms into columns encirclement, and the method is simple and reliable, precision is higher, can be used for the complex tasks such as the collaborative collection of multiple movement bodies.

Technical scheme: for achieving the above object, the technical solution used in the present invention is:

The present invention is that a kind of Multi-layered wound is formed into columns the geometric design method surrounded, and the target super ellipsoids face that is particularly well-suited under hyperquadric centre coordinate system to describe, expects that simple convex closed orbit and movable body are dynamic.

Consider hyperquadric centre coordinate system W_is={ o_is,x_is,y_is,z_isDescribe the i-th �� [1 ..., n] individual movable body is dynamic, wherein o_isFor the center in target super ellipsoids face corresponding to i-th movable body, z_isAxle points up. Under hyperquadric centre coordinate system, the motion of movable body can regard the motion of rigid body as, meets Newton's second law, is subject to the interference in known flow field, space simultaneously:

$\left\{\begin{array}{c}{\stackrel{\·}{p}}_i=v_i+f_{pi}(p_i,t)\\ {\stackrel{\·}{v}}_i=m_i{u}_i+f_{vi}(p_i,v_i)\end{array}\right.$

Wherein p_i=[p_ix,p_iy,p_iz]^TRepresent that i-th movable body is at W_isIn position coordinates, v_i=[v_ix,v_iy,v_iz]^TRepresent that i-th movable body is at W_isIn speed, m_iFor i-th movable body quality, u_i=[u_ix,u_iy,u_iz]^T��W_isIt is the control power input of i-th movable body, f_pi(p_i, t)=[f_pix(p_i,t),f_piy(p_i,t),f_piz(p_i,t)]^T��W_isThe velocity vector in representation space flow field, f_vi(p_i,v_i)=[f_vix(p_i,v_i),f_viy(p_i,v_i),f_viz(p_i,v_i)]^T��W_isThe celestial body gravitation being subject to when representing motion, i=1 ..., n. Such as, gravitation f in the equation of motion of satellite_vi(p_i,v_i) can be write asWherein �� is celestial body gravitational constant,For perturbation acceleration; It is diversion in the C-W equation of motion of star f_vi(p_i,v_i) can be write as $f_{vi}(p_i,v_i)=\left[\begin{array}{ccc}3{\mathrm{\ω}}^2& 0& 0\\ 0& 0& 0\\ 0& 0& -{\mathrm{\ω}}^2\end{array}\right]p_i+\left[\begin{array}{ccc}0& 2\mathrm{\ω}& 0\\ -2\mathrm{\ω}& 0& 0\\ 0& 0& 0\end{array}\right]v_i,$ Wherein �� is by the angular velocity moved along circular orbit of the star that is diversion. Fig. 1 is the conversion signal between global coordinate system and quadratic surface centre coordinate system.

Multiple movement bodies is in surrounding formation motion, and the information between movable body is requisite alternately, and we use directed graph hereDescribe, whereinFor set of node,Set for directed edge. If nodeThere is directed edge and arrive nodeThis means that i-th movable body can obtain the information of jth movable body, and jth movable body is the adjacent node of i-th movable body. The adjacent node set of i-th movable body is usedRepresent. We say nodeThere is a directed walk and arrive nodeNamely there is one group of directed edge in orderFrom nodeTo nodeWhereinIt isThe set of middle k+1 different nodes. If other all nodes arrive node such as through directed walk in directed graphThen nodeIt is referred to as overall situation accessible point. If nodeWith nodeExist and be bi-directionally connected limit, namely corresponding adjacency matrix A=[a_ij] in a_ij=a_ji=1, other a_ij=a_ji=0, corresponding information topology alternately is two-dimensional plot. If nodeTo nodeThere is unidirectional connection limit, namely corresponding adjacency matrix A=[a_ij] in a_ij=1, other a_ij=0, corresponding information topology alternately is directed graph. The Laplacian matrix of figure can be expressed as L=[l_ij], wherein l_ii=��_j��ia_ijAnd l_ij=-a_ij. In Fig. 2, left side is the mutual topologically corresponding two-dimensional plot of the information between 5 movable bodies (all of which node is overall situation accessible point), and right side is directed graph (its interior joint { ��₁,��₂,��₃Be overall situation accessible point). During design, we are once provide multiple movement bodies information interaction relation, then each moment movable body i laterAll constant, and correspondence two-way/directed graph contain the overall situation accessible point. In engineer applied, we can adopt the information that the mode that the mode of communication, the mode of sensing and two ways combine realizes between multiple movement bodies mutual flexibly according to practical situation.

The target super ellipsoids face of the movable body i for describing under quadratic surface centre coordinate systemOn the simple convex closed orbit of expectation Super ellipsoids can be usedWith plane P_i0Between intersection represent. The purpose of the present invention is exactly according to the information of adjacent motion body obtained, design the control power of each movable body make its move on respective target super ellipsoids surface expect simple convex closed orbit while keep certain formation between movable body.

In the present invention, for specifying in the following way along the formation position relation expected between each movable body that track moves on respective target super ellipsoids face: make D_iRepresent that i-th movable body does the rotating shaft direction of around the movement. p_ipFor the position coordinates projecting to rotating shaft of i-th movable body, make l_ipFor from p_ipPoint to p_iLine. ��_iT () is l_ipWith plane r_io_isx_isBetween angle, the angle that namely movable body rotates around rotating shaft. Broad sense anglec of rotation ��_i(��_i(t)) it is about rotation angle ��_iThe linear function of (t), i.e. ��_i(t)=b_i��_i(t)-��_i ^*, wherein b_i�� 0 and ��_i ^*Determine for constant and by formation. Expectation formation position relationship is kept to refer between each movable body:

��_i(��_i(t))-��_j(��_i(t))=0.

Shown in Fig. 3 be that three movable bodies move on concentric super ellipsoids surface, rotating shaft is identical each expects track, meets the line between the position of any two movable body and rotating shaft all the time in approximately the same plane simultaneously. In order to meet above-mentioned formation, can set that �� here_i(��_i)=��_i, i=1,2,3. It is that three movable bodies move on the concentration ellipse track on the ellipsoid of concentration surface being in proportion with delta formation shown in Fig. 4, �� here₁(��₁)=��₁,��₃(��₃)=��₃��

The design philosophy of the present invention is, the simple convex closed orbit of expectation on the target super ellipsoids face described, target super ellipsoids face and movable body is dynamically again represented under hyperquadric centre coordinate system under global coordinate system; Again target super ellipsoids face is expanded to about toroidal function f by the mode of compression with one heart_si(p_i) equivalent super ellipsoids face bunch, the regularity of curved surface determine the movable scope of i-th movable body. Design movable body is along super ellipsoids face normal vector (be called for short Surface Method vector) N_iOn control power so that be initially located at ��_iIn movable body move all the time in ��_iAnd distance (the being called for short distance of curved surface error) d to target super ellipsoids face_is(t) and the derivative to the time thereofReduce to 0, it is achieved logging in of target surface; Design movable body is along expecting track place planar process vector (being called for short planar process vector) B_iOn control power so that movable body is to expectation track place interplanar distance (being called for short plan range error) d_ip(t) and the derivative to the time thereofReduce to 0, it is achieved around the movement. When movable body motion expectation track on respective target super ellipsoids face, the motion of forming into columns of multiple movement bodies just deteriorates to the Position And Velocity that movable body moves along track and reaches unanimously. Obtain the information of adjacent motion body alternately according to information, design movable body is along direction of rotation T_iOn control power part make broad sense anglec of rotation ��_i(t) and derivative thereofReach unanimously to realize the formation campaign of multiple movement bodies.

Concretely, the method comprises the steps:

(1) by the target super ellipsoids face of i-th movable body under global coordinate system, expect that simple convex closed orbit and movable body dynamically again represent under hyperquadric centre coordinate system;

(2) target super ellipsoids face is expanded to the equivalent super ellipsoids face bunch about toroidal function;

(3) calculated the i-th movable body distance to target super ellipsoids face by toroidal function, design i-th movable body control power along the normal vector of target super ellipsoids face, it is achieved the curved surface of i-th movable body logs in;

(4) calculating i-th movable body to the distance expecting simple convex closed orbit plane, the control power along simple convex closed orbit planar process vector is expected on design i-th movable body edge, it is achieved the track of i-th is followed the tracks of;

(5) rotating shaft of design collaboration around the movement, calculates the i-th movable body anglec of rotation around rotating shaft, designs i-th movable body control power in direction of rotation, it is achieved the formation campaign of i-th movable body.

Wherein said super ellipsoids, track and movable body dynamically describe under hyperquadric centre coordinate system.

Described step (1) specifically includes following steps:

(11) the homogeneous coordinate transformation matrix between global coordinate system and hyperquadric centre coordinate system is set up;

(12) under hyperquadric centre coordinate system, redescribe target super ellipsoids face corresponding to each i-th movable body, expect that simple convex closed orbit and movable body are dynamic.

Described step (2) specifically includes following steps:

(21) by the mode Extended target super ellipsoids face of compression with one heart, target super ellipsoids face is expanded to one group of equivalence super ellipsoids face bunch;

(22) regularity according to curved surface, it is determined that the range of movement of i-th movable body;

(23) in range of movement, build the toroidal function of i-th movable body, make expand one group of equivalence super ellipsoids face bunch can be taken different values by toroidal function and represent.

Described step (3) specifically includes following steps:

(31) by the position of i-th movable body and corresponding toroidal function, the i-th movable body distance of curved surface error to target super ellipsoids face is calculated;

(32) by the derivative of distance of curved surface error over time, the variable quantity of distance of curved surface error is calculated;

(33) according to distance of curved surface error and the derivative to the time thereof, i-th movable body control power along the normal vector of target super ellipsoids face is designed.

Described step (4) specifically includes following steps:

(41) by the position of i-th movable body, the i-th movable body plan range error to the simple convex closed orbit plane of expectation is calculated;

(42) by the derivative of plan range error over time, the variable quantity of Calculation Plane range error;

(43) according to plan range error and the derivative to the time thereof, design i-th movable body control power along the simple convex closed orbit planar process vector of expectation.

Described step (5) specifically includes following steps:

(51) by target super ellipsoids face and the simple convex closed orbit plane of expectation, the rotating shaft of design collaboration around the movement, and initial rotation angle is specified;

(52) by the position of i-th movable body and speed, the i-th movable body anglec of rotation around rotating shaft is calculated;

(53) according to the functional relationship requiring to determine between the broad sense anglec of rotation and the anglec of rotation of forming into columns, the broad sense anglec of rotation and the derivative to the time thereof are calculated;

(54) information of the adjacent motion body obtained alternately by information, designs i-th movable body control power in direction of rotation.

Beneficial effect: Multi-layered wound provided by the invention form into columns surround geometric design method, have simple and reliable, precision is higher and is easy to the feature of practice, can be used for the complex tasks such as multiple movement bodies cooperative information collection.

Accompanying drawing explanation

Fig. 1 is that the super ellipsoids face, track and the movable body that describe under ordinate transform and hyperquadric centre coordinate system are dynamic;

Fig. 2 is the mutual topologically corresponding two-dimensional plot of information and directed graph;

The formation expected on the track signal that Fig. 3 is three concentric super ellipsoids surfaces, rotating shaft is identical;

Fig. 4 is that three movable bodies move in the concentric rail on concentric super ellipsoids surface with delta formation;

Fig. 5 is the equivalent ellipsoid bunch that concentric compand target ellipsoid obtains;

Fig. 6 is the geometry designs flow chart of Multi-layered wound formation envelope of motion.

Above figure has: o_IThe initial point of-global coordinate system; x_IThe x-axis of-global coordinate system; y_IThe y-axis of-global coordinate system; z_IThe z-axis of-global coordinate system; o_isThe initial point of-hyperquadric centre coordinate system; x_isThe x-axis of-hyperquadric centre coordinate system; y_isThe y-axis of-hyperquadric centre coordinate system; z_isThe z-axis of-hyperquadric centre coordinate system; Q_i-hyperquadric centre coordinate is tied to the transformation matrix of global coordinate system;-world coordinates is tied to the transformation matrix of hyperquadric centre coordinate system; p_iI-i-th movable body is position vector under global coordinate system; v_iI-i-th movable body is velocity vector under global coordinate system; p_i-i-th movable body is position vector under hyperquadric centre coordinate system; v_i-i-th movable body is velocity vector under hyperquadric centre coordinate system;1 movable body of-;2 movable bodies of-;3 movable bodies of-;4 movable bodies of-;5 movable bodies of-; D_iThe rotating shaft that-i-th movable body is corresponding; v₁The speed of 1 movable body of-; v₂The speed of 2 movable bodies of-; v₃The speed of 3 movable bodies of-; ��₀The length at Shang Diandaoqi center ,-target super ellipsoids face; N_i-vertical hyperspherical direction; The length of c-expansion; The length of-c-compression; ��_i-orbital;The curved surface that-expansion c obtains;The curved surface that-compression-c obtains.

Detailed description of the invention

Below in conjunction with accompanying drawing, the present invention is further described.

Fig. 6 is the design flow diagram of the present invention, is made up of module P1, P2, P3, P4, P5, P6 and P7, and each module is described below:

Part I: module P1

Owing to design control law of the present invention is for being that the super ellipsoids face, track and the movable body that describe under hyperquadric centre coordinate system are dynamic. And no matter be target super ellipsoids face and expectation track in reality, or the dynamic of movable body often provides under global coordinate system. Module P1 is for realizing the conversion between global coordinate system and hyperquadric centre coordinate system.

As shown in Figure 1 be translated by coordinate system and rotation realizes global coordinate system W_I={ o_I,x_I,y_I,z_IAnd hyperquadric centre coordinate system between conversion. By coordinate system transformation matrix Q_i, it is possible to the super ellipsoids face described under global coordinate system, track and movable body dynamic mapping are redescribed under hyperquadric centre coordinate system. Module P1 specifically follows these steps to realize:

The first step: by the initial point o of global coordinate system_IMove to the center o of target super ellipsoids_is, obtain coordinate translation matrix

$Q_{it}=\left[\begin{array}{ccc}{t}_{1ix}& t_{2ix}& t_{3ix}\\ t_{1iy}& t_{2iy}& t_{3iy}\\ t_{1iz}& t_{2iz}& t_{3iz}\end{array}\right].$

Rotate { the x of global coordinate system_I,y_I,z_IMake { the x of itself and hyperquadric centre coordinate system_is,y_is,z_isConsistent, obtain spin matrix

Q_ir=[r_1i,r_2i,r_2i]^T��

Calculate inertial coordinate and be tied to the coordinate system transformation matrix Q of orbital coordinate system_iAnd inverse matrix

$Q_i=\left[\begin{array}{cccc}{r}_{1i}& t_{1ix}& t_{2ix}& t_{3ix}\\ r_{2i}& t_{1iy}& t_{2iy}& t_{3iy}\\ r_{3i}& t_{1iz}& t_{2iz}& t_{3ix}\\ 0& 0& 0& 1\end{array}\right],$

$Q_i^{-1}=\left[\begin{array}{cccc}{r}_{1i}& r_{2i}& r_{3i}& -\left(t_{3ix}{r}_{1i}+t_{3iy}{r}_{2i}+t_{3iz}{r}_{3i}\right)\\ t_{1ix}& t_{1iy}& t_{1iz}& -\left(t_{3ix}{t}_{1ix}+t_{3iy}{t}_{1iy}+t_{3iz}{t}_{1iz}\right)\\ t_{2ix}& t_{2iy}& t_{2iz}& -\left(t_{3ix}{t}_{2ix}+t_{3iy}{t}_{2iy}+t_{3iz}{t}_{2iz}\right)\\ 0& 0& 0& 1\end{array}\right].$

Second step: by coordinate system transformation matrix Q_i, position coordinates p under global coordinate system_iI=[p_iIx,p_iIy,p_iIy]^TAnd speed v_iI=[v_iIx,v_iIy,v_iIy]^TJust can redescribing in hyperquadric centre coordinate system, computing formula is as follows:

$\left[\begin{array}{c}{p}_i\\ 1\end{array}\right]=Q_i^{-1}\left[\begin{array}{c}{p}_{iI}\\ 1\end{array}\right],$

$\left[\begin{array}{c}{v}_i\\ 1\end{array}\right]=Q_i^{-1}\left[\begin{array}{c}{v}_{iI}\\ 1\end{array}\right].$

By the position coordinates in hyperquadric centre coordinate system after converting, substituting into the descriptive equation of super ellipsoids under global coordinate system, we can obtain super ellipsoids in hyperquadric centre coordinate systemAnd withIntersecting plane P_i0Descriptive equation

$S_{i0}^2:{\left({\left(\frac{p_{ix}}{a_{1i}}\right)}^{\frac{2}{{\mathrm{\ϵ}}_{i2}}}+{\left(\frac{p_{iy}}{a_{2i}}\right)}^{\frac{2}{{\mathrm{\ϵ}}_{i2}}}\right)}^{\frac{{\mathrm{\ϵ}}_{i2}}{{\mathrm{\ϵ}}_{i1}}}+{\left(\frac{p_{iz}}{a_{3i}}\right)}^{\frac{2}{{\mathrm{\ϵ}}_{i1}}}=1,$

$P_{i0}:B_i\·(p_i-p_i^{*})=0$

Wherein, a_i1And a_i2It is that equatorial radius is (along x_isAnd y_isAxle), a_i3It is that polar radius is (along z_isAxle), ��_i1And ��_i2Respectively Dong-Xi and north-south index, B_iIt is planar process vector,It is plane P_i0In any one fixing point. And then, the expectation track on super ellipsoids surfaceSuper ellipsoids can be usedEquation and plane P_i0Equation connection list is shown

$\left\{\begin{array}{c}{\left({\left(\frac{p_{ix}}{a_{1i}}\right)}^{\frac{2}{{\mathrm{\ϵ}}_{i2}}}+{\left(\frac{p_{iy}}{a_{2i}}\right)}^{\frac{2}{{\mathrm{\ϵ}}_{i2}}}\right)}^{\frac{{\mathrm{\ϵ}}_{i2}}{{\mathrm{\ϵ}}_{i1}}}+{\left(\frac{p_{iz}}{a_{3i}}\right)}^{\frac{2}{{\mathrm{\ϵ}}_{i1}}}=1\\ B_i\·(p_i-p_i^{*})=0\end{array}\right..$

Part II: module P2

As it is shown in figure 5, the super ellipsoids face described under hyperquadric centre coordinate system?Near, willOn the every bit real number �� different along direction translation (compression and expansion) crossing this point vertical super ellipsoids face_i, we can obtain different super ellipsoids face (as:With). Module P2 specifically follows these steps to realize:

The first step,Near, willOn every bit along cross the vertical super ellipsoids face of this point direction (the method direction in super ellipsoids face) N_iTranslate different real number ��_iHypersphere after being expandedNamely

$S_{i\mathrm{\λ}}^2=(1+{\mathrm{\λ}}_i)S_{i0}^2;$

Second step, need to be met regularity conditions by the sphere after extending, and we select the movable scope �� of i-th_i-�� is met for all in space_i<��_i< the set of �� < �� point.

3rd step, due to set omega_iIn every bit broadly fall into ��_iIn one extension super ellipsoids face, we can at ��_iUpper structure toroidal function

$f_{is}\left(p_i\right)=1-\left({\left({\left(\frac{p_{ix}}{a_{1i}}\right)}^{\frac{2}{{\mathrm{\&epsiv;}}_{i2}}}+{\left(\frac{p_{iy}}{a_{2i}}\right)}^{\frac{2}{{\mathrm{\&epsiv;}}_{i2}}}\right)}^{\frac{{\mathrm{\&epsiv;}}_{i2}}{{\mathrm{\&epsiv;}}_{i1}}}+{\left(\frac{p_{iz}}{a_{3i}}\right)}^{\frac{2}{{\mathrm{\&epsiv;}}_{i1}}}\right).$

And then, ��_i�� can be expressed as_i={ p_i��W_oi|-��_i<f_is(p_i) < �� }, wherein ��_iFor each point on target super ellipsoids face to the beeline at its center.It is about toroidal function f_isAn equivalent super ellipsoids face, namelyThen f_is(p_i)=��_i; Whenf_is(p_i)=0.

Part III: module P3

Module P3 is used to design movable body along the control power part on the normal vector of super ellipsoids face, movable body is made to reduce the requirement to satisfied design to the distance of curved surface error in target super ellipsoids face, ensureing that movable body is all the time at movable range of motion, concrete design procedure is as follows simultaneously:

The first step: by toroidal function f_isCurrent location p with movable body_i, calculate the movable body distance of curved surface error d to target super ellipsoids face_is(t)

$d_{is}=f_{is}\left(p_i\right)-0=1-\left({\left({\left(\frac{p_{ix}}{a_{1i}}\right)}^{\frac{2}{{\mathrm{\&epsiv;}}_{i2}}}+{\left(\frac{p_{iy}}{a_{2i}}\right)}^{\frac{2}{{\mathrm{\&epsiv;}}_{i2}}}\right)}^{\frac{{\mathrm{\&epsiv;}}_{i2}}{{\mathrm{\&epsiv;}}_{i1}}}+{\left(\frac{p_{iz}}{a_{3i}}\right)}^{\frac{2}{{\mathrm{\&epsiv;}}_{i1}}}\right);$

Second step: by toroidal function f_isCalculate the normal vector N in super ellipsoids face_i

$N_i=-\frac{\&dtri;f_{is}}{\left|\right|\&dtri;f_{is}\left|\right|}.$

By N_iWith the position of movable body and speed, calculating distance of curved surface error d_is(t) derivative to the time

${\stackrel{\&CenterDot;}{d}}_{is}=-N_i\&CenterDot;(v_i+f_{pi});$

3rd step: design movable body control power along super ellipsoids Surface Method vector

$N_i\&CenterDot;u_i=F_{ni}=\frac{1}{m_i}\left({\mathrm{\&psi;}}_i\right(d_{is})+k_1{\stackrel{\&CenterDot;}{d}}_{is}+{\mathrm{\&Delta;}}_{fis})$

Wherein,

${\mathrm{\&psi;}}_i\left(d_{is}\right)=\tan \left(\frac{\mathrm{\&pi;}\left(2d_{is}-{\mathrm{\&epsiv;}}_i+{\mathrm{\&rho;}}_i\right)}{2\left({\mathrm{\&epsiv;}}_i+{\mathrm{\&rho;}}_i\right)}\right)-\tan \left(\frac{\mathrm{\&pi;}\left(-{\mathrm{\&epsiv;}}_i+{\mathrm{\&rho;}}_i\right)}{2\left({\mathrm{\&epsiv;}}_i+{\mathrm{\&rho;}}_i\right)}\right),$

${\mathrm{\&Delta;}}_{fis}=-N_i\&CenterDot;(f_{vi}+{\stackrel{\&CenterDot;}{f}}_{pi})+{\&dtri;}^2{f}_{is}(v_i+f_{pi})\&CenterDot;(v_i+f_{pi}),$

It is toroidal function f_isThe gloomy matrix majorization in sea, parameter k₁>0_,Function ��_i(d_is) it is for ensureing to be initially located at ��_iI-th movable body all the time at ��_iMiddle motion and finally moving in target super ellipsoids face.

Part IV: module P4

Module P4 is used to design movable body along the control power part on planar process vector so that movable body is to expecting track place plane P_i0Between plan range error reduce the requirement to satisfied design, concrete design procedure is as follows:

The first step: by the current location p of movable body_i, calculate movable body to expecting track place plane P_i0Between plan range error d_ip(t)

$d_{ip}\left(t\right)=B_i\&CenterDot;(p_i-p_i^{*});$

Second step: by plan range error d_ipT () and movable body are dynamic, Calculation Plane range error d_ip(t) derivative to the time

${\stackrel{\&CenterDot;}{d}}_{ip}=B_i\&CenterDot;(v_i+f_{pi});$

3rd step: design movable body is along the control power on planar process vector

$B_i\&CenterDot;u_i=F_{bi}=\frac{1}{m_i}(k_2{d}_{ip}+k_3{\stackrel{\&CenterDot;}{d}}_{ip}+{\mathrm{\&Delta;}}_{ip})$

Wherein, parameter k₂,k₃> 0,

Part V: module P5

Module P5 obtains the broad sense anglec of rotation �� of adjacent motion body alternately according to information_iAnd derivativeDesigning i-th movable body along the control power in direction of rotation in order to realize formation, concrete design procedure is as follows:

The first step: by Surface Method vector N_iWith plane normal vector B_i, calculating normal vector is B_iThe tangent point of contact p in plane and super ellipsoids face_*i=[p_*ix,p_*iy,p_*iz]^T, utilize the gloomy iterative method of newton-pressgang or secant method numerical solution following equations

N_i(p_*i)=B_i

Numerical solution point of contact p_*i. Connect point of contact p_*iWith zero o_isIt is rotating shaft. The direction of rotating shaft is defined as D_i=p_*i/||p_*i| |. Then, the position p at i-th movable body place is calculated_iPoint to its position coordinates p projecting to rotating shaft_ipVector l_ip(t)

$I_{ip}=p_i-\frac{p_i\&CenterDot;D_i}{\left|\right|D_i\left|\right|}{D}_i.$

Then, the initial angle �� that movable body rotates around rotating shaft_i(0) for l_ip(0) with plane r_io_isx_isBetween angle, computing formula is as follows:

${\mathrm{\&theta;}}_i\left(0\right)=\frac{\mathrm{\&pi;}}{2}-arccos\left(\frac{l_{ip}}{\left|\right|l_{ip}\left|\right|}\&CenterDot;{\&lsqb;0,1,0\&rsqb;}^T\right).$

Second step: calculate the rotation angle �� of movable body_i(t)

${\mathrm{\&theta;}}_i={\&Integral;}_0^t{T}_i\left(\mathrm{\&tau;}\right)\&CenterDot;\left(v_i\left(\mathrm{\&tau;}\right)+f_{pi}\left(p_i\left(\mathrm{\&tau;}\right),\mathrm{\&tau;}\right)\right)d\mathrm{\&tau;}+{\mathrm{\&theta;}}_i\left(0\right)$

Wherein, T_iT () is direction of rotation, expression formula is as follows:

$T_i=\frac{D_i\&times;p_i}{\left|\right|l_{ip}\left|\right|\left|\right|D_i\&times;p_i\left|\right|}.$

3rd step: rotation angle ��_iT () and expectation formation, define broad sense anglec of rotation ��_iParameter b in (t)_i�� 0 and ��_i ^*, calculate broad sense anglec of rotation ��_i(t)

��_i=b_i��_i-��_i ^*��

Then, the broad sense anglec of rotation derivative to the time is calculated

${\stackrel{\&CenterDot;}{\mathrm{\&xi;}}}_i=b_i{T}_i\&CenterDot;(v_i+f_{pi});$

4th step: according to the broad sense anglec of rotation of the obtained alternately adjacent motion body of information and derivative thereof, design movable body is along the control power in direction of rotation

$T_i\&CenterDot;u_i=F_{ti}=-\frac{1}{m_i{b}_i}\left(k_4\underset{j=1}{\overset{n}{\&Sigma;}}{a}_{ij}\left(\stackrel{\&OverBar;}{{\mathrm{\&xi;}}_i}-\stackrel{\&OverBar;}{{\mathrm{\&xi;}}_j}\right)+k_5\underset{j=1}{\overset{n}{\&Sigma;}}{a}_{ij}\left(\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{{\mathrm{\&xi;}}_i}}-\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{{\mathrm{\&xi;}}_j}}\right)+a_0{k}_6\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{{\mathrm{\&xi;}}_i}}+{\mathrm{\&Delta;}}_{\mathrm{\&xi;}i}\right)$

Wherein,

$\stackrel{\&OverBar;}{{\mathrm{\&xi;}}_i}={\mathrm{\&xi;}}_i-a_0{\&Integral;}_0^t{\stackrel{\&CenterDot;}{\mathrm{\&xi;}}}^{*}\left(\mathrm{\&tau;}\right)d\mathrm{\&tau;}$

For desired broad sense angular velocity of rotation; a₀�� 0,1}, work as a₀Represent when=1 in formation task and angular velocity of rotation is required, otherwise a₀=0;

${\mathrm{\&Delta;}}_{i\mathrm{\&xi;}}=T_i\&CenterDot;(f_{vi}+{\stackrel{\&CenterDot;}{f}}_{pi})+{\stackrel{\&CenterDot;}{T}}_i\&CenterDot;(v_i+f_{pi}),$

${\stackrel{\&CenterDot;}{T}}_i=\frac{D_i\&times;\left(v_i+f_{pi}\right)}{\left|\right|l_{ip}\left|\right|\left|\right|D_i\&times;p_i\left|\right|}-\frac{\left(\left(D_i\&times;p_i\right)\&CenterDot;\left(D_i\&times;\left(v_i+f_{pi}\right)\right)\right)D_i\&times;p_i}{\left|\right|l_{ip}\left|\right|\left|\right|D_i\&times;p_i|{|}^3}-\frac{\left(l_{ip}\&CenterDot;\left(v_i+f_{pi}-\frac{\left(v_i+f_{pi}\right)\&CenterDot;D_i}{\left|\right|D_i\left|\right|}\right)\right)D_i\&times;p_i}{\left|\right|l_{ip}\left|{|}^3\right||D_i\&times;p_i||},$

Control parameter k₄,k₆> 0; If the information commutative Topology containing overall situation accessible point is two-way, control parameter k₅Select the constant more than 0; If the information commutative Topology containing overall situation accessible point is unidirectional, control parameter k₅Need to meet $k_5>\underset{{\mathrm{\&mu;}}_i\&NotEqual;0}{max}\left(\frac{k_6}{\mathrm{Re}\left({\mathrm{\&mu;}}_i\right)}+\frac{k_6{\left(\mathrm{Im}\right({\mathrm{\&mu;}}_i\left)\right)}^2+\left|\mathrm{Im}\left({\mathrm{\&mu;}}_i\right)\right|\sqrt{{\left(k_6\mathrm{Im}\right({\mathrm{\&mu;}}_i\left)\right)}^2-4k_4\mathrm{Re}\left({\mathrm{\&mu;}}_i\right)({\left(\mathrm{Re}\right({\mathrm{\&mu;}}_i\left)\right)}^2+{\left(\mathrm{Im}\right({\mathrm{\&mu;}}_i\left)\right)}^2)}}{-2\mathrm{Re}\left({\mathrm{\&mu;}}_i\right)({\left(\mathrm{Re}\right({\mathrm{\&mu;}}_i\left)\right)}^2+{\left(\mathrm{Im}\right({\mathrm{\&mu;}}_i\left)\right)}^2)}\right)$ Wherein, ��_i, i=1,2 ..., n, is the eigenvalue of-L.

Part VI: module P6

The result of module P6 integration module P3, P4 and P5 calculates the control power input of movable body, completes the motor control of movable body, and concrete realizes according to following steps:

The first step: controls power part and module P5 that the movable body that integration module P3 designs control power part along Surface Method vector, module P4 design along planar process vector design along the control power part in direction of rotation, and connection row solve the control power of final each movable body and input u_i

$u_i={\left[\begin{array}{c}{N}_i^T\\ B_i^T\\ T_i^T\end{array}\right]}^{-1}\left[\begin{array}{c}{F}_{ni}\\ F_{bi}\\ F_{ti}\end{array}\right];$

Second step: the control power of movable body is inputted and is sent to slave computer in the form of a command by host computer, the servosystem of movable body complete the motor control to movable body, and return to module P3.

The above is only the preferred embodiment of the present invention; it is noted that, for those skilled in the art; under the premise without departing from the principles of the invention, it is also possible to make some improvements and modifications, these improvements and modifications also should be regarded as protection scope of the present invention.

Claims (7)

1. the geometric design method that a Multi-layered wound formation is surrounded, it is characterized in that: Multi-layered wound form into columns the target super ellipsoids face in surrounding, target super ellipsoids face expects that simple convex closed orbit and movable body dynamically describe under hyperquadric centre coordinate system, target super ellipsoids face expands to the equivalent super ellipsoids face bunch about toroidal function by the mode of compression with one heart, the regularity of curved surface determine the movable scope of i-th movable body.

2. the geometric design method that Multi-layered wound according to claim 1 formation is surrounded, it is characterised in that: the method comprises the steps:

(1) by the target super ellipsoids face of i-th movable body under global coordinate system, expect that simple convex closed orbit and movable body dynamically again represent under hyperquadric centre coordinate system;

(2) target super ellipsoids face is expanded to the equivalent super ellipsoids face bunch about toroidal function;

(3) calculated the i-th movable body distance to target super ellipsoids face by toroidal function, design i-th movable body control power along the normal vector of target super ellipsoids face, it is achieved the curved surface of i-th movable body logs in;

(4) calculating i-th movable body to the distance expecting simple convex closed orbit plane, the control power along simple convex closed orbit planar process vector is expected on design i-th movable body edge, it is achieved the track of i-th is followed the tracks of;

(5) rotating shaft of design collaboration around the movement, calculates the i-th movable body anglec of rotation around rotating shaft, designs i-th movable body control power in direction of rotation, it is achieved the formation campaign of i-th movable body.

3. the geometric design method that Multi-layered wound according to claim 2 formation is surrounded, it is characterised in that: described step (1) specifically includes following steps:

(11) the homogeneous coordinate transformation matrix between global coordinate system and hyperquadric centre coordinate system is set up;

(12) under hyperquadric centre coordinate system, redescribe target super ellipsoids face corresponding to each i-th movable body, expect that simple convex closed orbit and movable body are dynamic.

4. the geometric design method that Multi-layered wound according to claim 2 formation is surrounded, it is characterised in that: described step (2) specifically includes following steps:

(21) by the mode Extended target super ellipsoids face of compression with one heart, target super ellipsoids face is expanded to one group of equivalence super ellipsoids face bunch;

(22) regularity according to curved surface, it is determined that the range of movement of i-th movable body;

(23) in range of movement, build the toroidal function of i-th movable body, make expand one group of equivalence super ellipsoids face bunch can be taken different values by toroidal function and represent.

5. the geometric design method that Multi-layered wound according to claim 2 formation is surrounded, it is characterised in that: described step (3) specifically includes following steps:

(31) by the position of i-th movable body and corresponding toroidal function, the i-th movable body distance of curved surface error to target super ellipsoids face is calculated;

(32) by the derivative of distance of curved surface error over time, the variable quantity of distance of curved surface error is calculated;

(33) according to distance of curved surface error and the derivative to the time thereof, i-th movable body control power along the normal vector of target super ellipsoids face is designed.

6. the geometric design method that Multi-layered wound according to claim 2 formation is surrounded, it is characterised in that: described step (4) specifically includes following steps:

(41) by the position of i-th movable body, the i-th movable body plan range error to the simple convex closed orbit plane of expectation is calculated;

(42) by the derivative of plan range error over time, the variable quantity of Calculation Plane range error;

(43) according to plan range error and the derivative to the time thereof, design i-th movable body control power along the simple convex closed orbit planar process vector of expectation.

7. the geometric design method that Multi-layered wound according to claim 2 formation is surrounded, it is characterised in that: described step (5) specifically includes following steps:

(51) by target super ellipsoids face and the simple convex closed orbit plane of expectation, the rotating shaft of design collaboration around the movement, and initial rotation angle is specified;

(52) by the position of i-th movable body and speed, the i-th movable body anglec of rotation around rotating shaft is calculated;

(53) according to the functional relationship requiring to determine between the broad sense anglec of rotation and the anglec of rotation of forming into columns, the broad sense anglec of rotation and the derivative to the time thereof are calculated;

(54) information of the adjacent motion body obtained alternately by information, designs i-th movable body control power in direction of rotation.