CN104326074A - CAM matrix-based underwater robot vectored thrust distribution method - Google Patents

Abstract

The invention discloses a CAM matrix-based underwater robot vectored thrust distribution method, which is used for solving the technical problem that the direction control of the conventional underwater robot vectored thrust distribution method is single. Aiming at a multi-degree of freedom vectored thrust underwater robot, the technical solution directly associates the execution result of each multi-degree of freedom vectored thruster control instruction with contribution to the six degrees of freedom of the underwater robot made by the multi-degree of freedom vectored thruster control instruction, gives a distribution contribution coefficient, and generates a contribution coefficient vector related to the six degrees of freedom of the underwater robot for each degree of freedom control instruction of each thruster. When a controller gives each degree of freedom control instruction to the underwater robot, the inner product of the vector formed by each degree of freedom demand instruction of the underwater robot and the contribution coefficient vector corresponding to the degree of freedom of each thruster is worked out, and the result is the control instruction of the thruster in the degree of freedom direction. The method can effectively distribute the control instructions of each degree of freedom of the underwater robot to each multi-degree of freedom thruster.

Description

Based on the under-water robot vectored thrust distribution method of CAM matrix

Technical field

The present invention relates to a kind of under-water robot vectored thrust distribution method, particularly relate to a kind of under-water robot vectored thrust distribution method based on CAM matrix.

Background technology

Under-water robot, particularly Cast tube, its motion control relies on propelling unit to realize.In order to meet the minimum maneuverability requirement of under-water robot, the minimum motion that should possess 3 degree of freedom of under-water robot, and retreat, snorkeling and turn bow.The freedom of motion of most under-water robot is no less than 4, except 3 degree of freedom above, also comprise lateral translational movement.

Document " Optimization of Thrust Allocation in the Propulsion System of an Underwater Vehicle.Int.J.Appl.Math.Comput.Sci2004; Vol.14; No.4:461 – 467 " discloses a kind of submarine navigation device thrust optimizing distribution method, and the method effectively can solve the motion control thrust assignment problem under fixing single degree of freedom propelling unit combination.Because propeller thrust direction is fixed, only possess the single degree of freedom vector propelling that both forward and reverse directions controls, fixing thrust allocation scheme thus can be used effectively to configure thrust and export.

Consider if each propelling unit possesses the ability of two and plural degree of freedom output vector thrust, so input for specific motion control, thrust distribution combination under being driven by multiple vector propeller has infinite multiple, adopts fixing thrust allocation scheme cannot adapt to different motion control requirements at all.How under-water robot controller and multiple degree of freedom vector actuating unit effectively being coupled together, realizing effective distribution of vectored thrust under specific control overflow, is the important topic that Cast tube must be researched and solved.

Summary of the invention

In order to overcome the single deficiency of existing under-water robot vectored thrust distribution method direction controlling, the invention provides a kind of under-water robot vectored thrust distribution method based on CAM matrix.The method advances Cast tube for multiple degree of freedom vector, by the execution result of the control command of each multiple degree of freedom vector propeller directly and its contribution of under-water robot six-freedom degree is connected, give to distribute certain contribution coefficient, then can form a contribution coefficient relevant to under-water robot six-freedom degree for each angle of rake each degree of freedom control command vectorial.When controller assigns each degree of freedom control command to under-water robot, contribution coefficient vector corresponding with each propelling unit degree of freedom for the vector of each for under-water robot degree of freedom requirement command composition is done inner product, and result is the control command of propelling unit on this degree of freedom direction.The control command of each for under-water robot degree of freedom can be allocated on each multiple degree of freedom propelling unit by the inventive method effectively.

The technical solution adopted for the present invention to solve the technical problems is: a kind of under-water robot vectored thrust distribution method based on CAM matrix, is characterized in adopting following steps:

Six faces of step one, selection under-water robot outline form cuboids, and the origin of coordinates O using the geometric centre of this cuboid as satellite system of axes, OX axle is indulged in the plane of symmetry at cuboid, perpendicular to robot front end face, points to working direction; The OZ axle cuboid that coexists is indulged in the plane of symmetry, vertically with OX axle points to top; OY axle, perpendicular to ZOX plane, forms right hand rectangular coordinate system with OX axle and OZ axle.

Step 2, under-water robot use Γ respectively three translation freedoms demands _x, Γ _yand Γ _zrepresent, use Ω respectively three rotary freedom demands _x, Ω _yand Ω _zrepresent, then the vector representation of degree of freedom requirement command is: D=[Γ _xΓ _yΓ _zΩ _xΩ _yΩ _z] ^t;

Step 3, under-water robot have four horizontal propellers and two vertical pusher, four horizontal propeller axis are in same plane, and parallel with XOY plane, the axis of left front horizontal propeller 1, right front horizontal propeller 4, left back horizontal propeller 6 and right back horizontal propeller 9 and the angle of ZOX plane are respectively γ _fL, γ _fR, γ _aLand γ _aR, the installation attachment point P of left front horizontal propeller 1, right front horizontal propeller 4, left back horizontal propeller 6 and right back horizontal propeller 9 _fL, P _fR, P _aLand P _aRcoordinate in satellite system of axes is respectively (X _fL, Y _fL, Z _fL), (X _fR, Y _fR, Z _fR), (X _aL, Y _aL, Z _aL) and (X _aR, Y _aR, Z _aR); Two vertical pusher 5 axis in same plane, and with YOZ plane parallel, the angle of vertical left (CL) propelling unit and vertical right (CR) propelling unit and ZOX plane is respectively γ _cL, γ _cR, the installation attachment point P of two each vertical pusher 5 _cLand P _cRcoordinate in satellite system of axes is (X _cL, Y _cL, Z _cL) and (X _cR, Y _cR, Z _cR).

Actuating spindle 3 before left front horizontal propeller 1 and right front horizontal propeller 4 and propelling vector is connected, adopts dynamic seal mode to insert front vector and advance servomechanism 2; Actuating spindle 8 after left back horizontal propeller 6 and right back horizontal propeller 9 and propelling vector is connected, adopts dynamic seal mode to insert rear vector and advance servomechanism 7.Under-water robot is according to control command, and the rotation advancing servomechanism 2 and rear vector to advance the coordinated signals of servomechanism 7 to realize thrust vectoring by front vector exports, the anglec of rotation δ of actuating spindle 3 before propelling vector _frepresent, the anglec of rotation δ of actuating spindle 8 after propelling vector _arepresent.

According to above definition, left front horizontal propeller 1 in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{FL}}^x=\cos {\mathrm{\γ}}_{\mathrm{FL}}\·\cos {\mathrm{\δ}}_f\\ F_{\mathrm{FL}}^y=-\cos {\mathrm{\γ}}_{\mathrm{FL}}\·\sin {\mathrm{\δ}}_f\\ F_{\mathrm{FL}}^z=\sin {\mathrm{\γ}}_{\mathrm{FL}}\end{array}\right.$

Right front horizontal propeller 4 in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{FR}}^x=\cos {\mathrm{\γ}}_{\mathrm{FR}}\·\cos {\mathrm{\δ}}_f\\ F_{\mathrm{FR}}^y=-\cos {\mathrm{\γ}}_{\mathrm{FR}}\·\sin {\mathrm{\δ}}_f\\ F_{\mathrm{FR}}^z=-\sin {\mathrm{\γ}}_{\mathrm{FR}}\end{array}\right.$

Left back horizontal propeller 6 in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{AL}}^x=-\cos {\mathrm{\γ}}_{\mathrm{AL}}\·\cos {\mathrm{\δ}}_a\\ F_{\mathrm{AL}}^y=-\cos {\mathrm{\γ}}_{\mathrm{AL}}\·\sin {\mathrm{\δ}}_a\\ F_{\mathrm{AL}}^z=\sin {\mathrm{\γ}}_{\mathrm{AL}}\end{array}\right.$

Right back horizontal propeller 9 in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{AR}}^x=-\cos {\mathrm{\γ}}_{\mathrm{AR}}\·\cos {\mathrm{\δ}}_a\\ F_{\mathrm{AR}}^y=-\cos {\mathrm{\γ}}_{\mathrm{AR}}\·\sin {\mathrm{\δ}}_a\\ F_{\mathrm{AR}}^z=-\sin {\mathrm{\γ}}_{\mathrm{AR}}\end{array}\right.$

Two vertical pusher 5 are expressed as at the contribution coefficient vector in three translation freedoms directions with wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{CL}}^x=\cos {\mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^y=-\cos {\mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^z=\sin {\mathrm{\γ}}_{\mathrm{CL}}\end{array}\right.$

$\left\{\begin{array}{c}{F}_{\mathrm{CR}}^x=\cos {\mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^y=-\cos {\mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^z=-\sin {\mathrm{\γ}}_{\mathrm{CR}}\end{array}\right.$

The contribution coefficient vector of left front horizontal propeller 1 in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of right front horizontal propeller 4 in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of left back horizontal propeller 6 in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of right back horizontal propeller 9 in three rotary freedom directions be calculated as follows:

Two vertical pusher 5 are expressed as at the contribution coefficient vector in three rotary freedom directions with be calculated as follows:

The control dispenser matrix L finally obtained by each angle of rake contribution coefficient vector is:

$L=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^y& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]$

Step 4, obtained m angle of rake thrust command vector T=[T of m × 1 by degree of freedom requirement command vector D and the result of the inner product LD controlling dispenser matrix L _fLt _fRt _aLt _aRt _cLt _cR] ^t, be calculated as follows formula, complete thrust thus and distribute:

$\left[\begin{array}{c}{T}_{\mathrm{FL}}\\ T_{\mathrm{FR}}\\ T_{\mathrm{AL}}\\ T_{\mathrm{AR}}\\ T_{\mathrm{CL}}\\ T_{\mathrm{CR}}\end{array}\right]=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^y& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]\left[\begin{array}{c}{\mathrm{\Γ}}_x\\ {\mathrm{\Γ}}_y\\ {\mathrm{\Γ}}_z\\ {\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_z\end{array}\right].$

Wherein, m is angle of rake number.

The invention has the beneficial effects as follows: the method advances Cast tube for multiple degree of freedom vector, by the execution result of the control command of each multiple degree of freedom vector propeller directly and its contribution of under-water robot six-freedom degree is connected, give to distribute certain contribution coefficient, then can form a contribution coefficient relevant to under-water robot six-freedom degree for each angle of rake each degree of freedom control command vectorial.When controller assigns each degree of freedom control command to under-water robot, contribution coefficient vector corresponding with each propelling unit degree of freedom for the vector of each for under-water robot degree of freedom requirement command composition is done inner product, and result is the control command of propelling unit on this degree of freedom direction.The control command of each for under-water robot degree of freedom can be allocated on each multiple degree of freedom propelling unit by the inventive method effectively.

Below in conjunction with the drawings and specific embodiments, the present invention is elaborated.

Accompanying drawing explanation

Fig. 1 is under-water robot vectored thrust set-up diagram in the under-water robot vectored thrust distribution method that the present invention is based on CAM matrix.

In figure, the left front horizontal propeller of 1-, before 2-, vector advances servomechanism, actuating spindle before 3-propelling vector, the right front horizontal propeller of 4-, 5-vertical pusher, the left back horizontal propeller of 6-, after 7-, vector advances servomechanism, actuating spindle after 8-propelling vector, the right back horizontal propeller of 9-.

Detailed description of the invention

With reference to Fig. 1.The under-water robot vectored thrust distribution method concrete steps that the present invention is based on CAM matrix are as follows:

1. set up propelling unit layout benchmark.

Underwater robot propeller layout with " satellite system of axes " for benchmark is determined.In the present invention, satellite system of axes is defined as, select six faces of under-water robot outline to form cuboid, the origin of coordinates O using the geometric centre of this cuboid as satellite system of axes, OX axle is indulged in the plane of symmetry at cuboid, perpendicular to robot front end face, point to working direction; The OZ axle cuboid that coexists is indulged in the plane of symmetry, vertically with OX axle points to top; OY axle, perpendicular to ZOX plane, forms right hand rectangular coordinate system with OX axle and OZ axle.

2. calculate degree of freedom requirement command vector.

Under-water robot uses Γ respectively three translation freedoms demands _x, Γ _yand Γ _zrepresent, use Ω respectively three rotary freedom demands _x, Ω _yand Ω _zrepresent, then degree of freedom requirement command vector can be expressed as: D=[Γ _xΓ _yΓ _zΩ _xΩ _yΩ _z] ^t;

3. calculate propeller control dispenser matrix.

Artificial four horizontal propellers of design underwater, add two vertical pusher layouts, four horizontal propeller axis are in same plane, and parallel with XOY plane, the angle of left front horizontal propeller 1, right front horizontal propeller 4, left back horizontal propeller 6, right back horizontal propeller 9 four propeller axis and ZOX plane is γ _fL, γ _fR, γ _aLand γ _aR, and four propelling units install attachment point P _fL, P _fR, P _aLand P _aRcoordinate in satellite system of axes is respectively (X _fL, Y _fL, Z _fL), (X _fR, Y _fR, Z _fR), (X _aL, Y _aL, Z _aL) and (X _aR, Y _aR, Z _aR); Two vertical pusher 5 axis in same plane, and with YOZ plane parallel, the angle of vertical left (CL) and vertical right (CR) two propelling units and ZOX plane is γ _cL, γ _cR, two propelling units install attachment point P _cLand P _cRcoordinate in satellite system of axes is (X _cL, Y _cL, Z _cL) and (X _cR, Y _cR, Z _cR).

Actuating spindle 3 before left front horizontal propeller 1 and right front horizontal propeller 4 and propelling vector is connected, adopts dynamic seal mode to insert front vector and advance servomechanism 2; Actuating spindle 8 after left back horizontal propeller 6 and right back horizontal propeller 9 and propelling vector is connected, adopts dynamic seal mode to insert rear vector and advance servomechanism 7.Two pairs, front and back horizontal propellers advance vector controlled axle to be connected into respectively by one and are integrated, under-water robot is according to control command, advance the control of servomechanism to link to realize the rotation of thrust vectoring to export by vector, advance the anglec of rotation δ of actuating spindle 3 before vector _frepresent, the anglec of rotation δ of actuating spindle 8 after propelling vector _arepresent.

According to above definition, left front horizontal propeller 1 can be expressed as at the contribution coefficient vector in three translation freedoms directions wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{FL}}^x=\cos {\mathrm{\γ}}_{\mathrm{FL}}\·\cos {\mathrm{\δ}}_f\\ F_{\mathrm{FL}}^y=-\cos {\mathrm{\γ}}_{\mathrm{FL}}\·\sin {\mathrm{\δ}}_f\\ F_{\mathrm{FL}}^z=\sin {\mathrm{\γ}}_{\mathrm{FL}}\end{array}\right.$

Right front horizontal propeller 4 can be expressed as at the contribution coefficient vector in three translation freedoms directions wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{FR}}^x=\cos {\mathrm{\γ}}_{\mathrm{FR}}\·\cos {\mathrm{\δ}}_f\\ F_{\mathrm{FR}}^y=-\cos {\mathrm{\γ}}_{\mathrm{FR}}\·\sin {\mathrm{\δ}}_f\\ F_{\mathrm{FR}}^z=-\sin {\mathrm{\γ}}_{\mathrm{FR}}\end{array}\right.$

Left back horizontal propeller 6 can be expressed as at the contribution coefficient vector in three translation freedoms directions wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{AL}}^x=-\cos {\mathrm{\γ}}_{\mathrm{AL}}\·\cos {\mathrm{\δ}}_a\\ F_{\mathrm{AL}}^y=-\cos {\mathrm{\γ}}_{\mathrm{AL}}\·\sin {\mathrm{\δ}}_a\\ F_{\mathrm{AL}}^z=\sin {\mathrm{\γ}}_{\mathrm{AL}}\end{array}\right.$

Right back horizontal propeller 9 can be expressed as at the contribution coefficient vector in three translation freedoms directions wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{AR}}^x=-\cos {\mathrm{\γ}}_{\mathrm{AR}}\·\cos {\mathrm{\δ}}_a\\ F_{\mathrm{AR}}^y=-\cos {\mathrm{\γ}}_{\mathrm{AR}}\·\sin {\mathrm{\δ}}_a\\ F_{\mathrm{AR}}^z=-\sin {\mathrm{\γ}}_{\mathrm{AR}}\end{array}\right.$

Two vertical pusher 5 can be expressed as at the contribution coefficient vector in three translation freedoms directions with wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{CL}}^x=\cos {\mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^y=-\cos {\mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^z=\sin {\mathrm{\γ}}_{\mathrm{CL}}\end{array}\right.$

$\left\{\begin{array}{c}{F}_{\mathrm{CR}}^x=\cos {\mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^y=-\cos {\mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^z=-\sin {\mathrm{\γ}}_{\mathrm{CR}}\end{array}\right.$

The contribution coefficient vector of left front horizontal propeller 1 in three rotary freedom directions be calculated as follows

The contribution coefficient vector of right front horizontal propeller 4 in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of left back horizontal propeller 6 in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of right back horizontal propeller 9 in three rotary freedom directions be calculated as follows:

Two vertical pusher 5 can be expressed as at the contribution coefficient vector in three rotary freedom directions with be calculated as follows:

Then the final control dispenser matrix L obtained by each angle of rake contribution coefficient vector is:

$L=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^y& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]$

4. calculate propeller thrust instruction vector.

M angle of rake thrust command vector T=[T of m × 1 is obtained by degree of freedom requirement command vector D and the result of the inner product LD controlling dispenser matrix L _fLt _fRt _aLt _aRt _cLt _cR] ^t, be calculated as follows formula, complete thrust thus and distribute:

$\left[\begin{array}{c}{T}_{\mathrm{FL}}\\ T_{\mathrm{FR}}\\ T_{\mathrm{AL}}\\ T_{\mathrm{AR}}\\ T_{\mathrm{CL}}\\ T_{\mathrm{CR}}\end{array}\right]=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^y& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]\left[\begin{array}{c}{\mathrm{\Γ}}_x\\ {\mathrm{\Γ}}_y\\ {\mathrm{\Γ}}_z\\ {\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_z\end{array}\right].$

Wherein, m is angle of rake number.

Claims (1)

1., based on a under-water robot vectored thrust distribution method for CAM matrix, it is characterized in that comprising the following steps:

Six faces of step one, selection under-water robot outline form cuboids, and the origin of coordinates O using the geometric centre of this cuboid as satellite system of axes, OX axle is indulged in the plane of symmetry at cuboid, perpendicular to robot front end face, points to working direction; The OZ axle cuboid that coexists is indulged in the plane of symmetry, vertically with OX axle points to top; OY axle, perpendicular to ZOX plane, forms right hand rectangular coordinate system with OX axle and OZ axle;

Step 2, under-water robot use Γ respectively three translation freedoms demands _x, Γ _yand Γ _zrepresent, use Ω respectively three rotary freedom demands _x, Ω _yand Ω _zrepresent, then the vector representation of degree of freedom requirement command is:

D＝[Γ _xΓ _yΓ _zΩ _xΩ _yΩ _z] ^T；

Step 3, under-water robot have four horizontal propellers and two vertical pusher, four horizontal propeller axis are in same plane, and parallel with XOY plane, left front horizontal propeller (1), right front horizontal propeller (4), left back horizontal propeller (6) and the axis of right back horizontal propeller (9) and the angle of ZOX plane are respectively γ _fL, γ _fR, γ _aLand γ _aR, the installation attachment point P of left front horizontal propeller (1), right front horizontal propeller (4), left back horizontal propeller (6) and right back horizontal propeller (9) _fL, P _fR, P _aLand P _aRcoordinate in satellite system of axes is respectively (X _fL, Y _fL, Z _fL), (X _fR, Y _fR, Z _fR), (X _aL, Y _aL, Z _aL) and (X _aR, Y _aR, Z _aR); Two vertical pusher 5 axis in same plane, and with YOZ plane parallel, the angle of vertical left (CL) propelling unit and vertical right (CR) propelling unit and ZOX plane is respectively γ _cL, γ _cR, the installation attachment point P of two each vertical pusher 5 _cLand P _cRcoordinate in satellite system of axes is (X _cL, Y _cL, Z _cL) and (X _cR, Y _cR, Z _cR);

Actuating spindle (3) before left front horizontal propeller (1) and right front horizontal propeller (4) and propelling vector is connected, adopts dynamic seal mode to insert front vector and advance servomechanism (2); Actuating spindle (8) after left back horizontal propeller (6) and right back horizontal propeller (9) and propelling vector is connected, adopts dynamic seal mode to insert rear vector and advance servomechanism (7); Under-water robot is according to control command, the rotation advancing servomechanism (2) and rear vector to advance the coordinated signals of servomechanism (7) to realize thrust vectoring by front vector exports, the anglec of rotation δ of actuating spindle (3) before propelling vector _frepresent, the anglec of rotation δ of actuating spindle (8) after propelling vector _arepresent;

According to above definition, left front horizontal propeller (1) in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{FL}}^x={\cos \mathrm{\γ}}_{\mathrm{FL}}\·{\cos \mathrm{\δ}}_f\\ F_{\mathrm{FL}}^y=-{\cos \mathrm{\γ}}_{\mathrm{FL}}\·{\sin \mathrm{\δ}}_f\\ F_{\mathrm{FL}}^z={\sin \mathrm{\γ}}_{\mathrm{FL}}\end{array}\right.$

Right front horizontal propeller (4) in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{FR}}^x={\cos \mathrm{\γ}}_{\mathrm{FR}}\·{\cos \mathrm{\δ}}_f\\ F_{\mathrm{FR}}^y=-{\cos \mathrm{\γ}}_{\mathrm{FR}}\·{\sin \mathrm{\δ}}_f\\ F_{\mathrm{FR}}^z={-\sin \mathrm{\γ}}_{\mathrm{FR}}\end{array}\right.$

Left back horizontal propeller (6) in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{AL}}^x={-\cos \mathrm{\γ}}_{\mathrm{AL}}\·{\cos \mathrm{\δ}}_a\\ F_{\mathrm{AL}}^y=-{\cos \mathrm{\γ}}_{\mathrm{AL}}\·{\sin \mathrm{\δ}}_a\\ F_{\mathrm{AL}}^z={\sin \mathrm{\γ}}_{\mathrm{AL}}\end{array}\right.$

Right back horizontal propeller (9) in the contribution coefficient vector representation in three translation freedoms directions is wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{AR}}^x=-{\cos \mathrm{\γ}}_{\mathrm{AR}}\·{\cos \mathrm{\δ}}_a\\ F_{\mathrm{AR}}^y=-{\cos \mathrm{\γ}}_{\mathrm{AR}}\·{\sin \mathrm{\δ}}_a\\ F_{\mathrm{AR}}^z={-\sin \mathrm{\γ}}_{\mathrm{AR}}\end{array}\right.$

Two vertical pusher 5 are expressed as at the contribution coefficient vector in three translation freedoms directions with wherein:

$\left\{\begin{array}{c}{F}_{\mathrm{CL}}^x={\cos \mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^y={-\cos \mathrm{\γ}}_{\mathrm{CL}}\\ F_{\mathrm{CL}}^z={\sin \mathrm{\γ}}_{\mathrm{CL}}\end{array}\right.$

$\left\{\begin{array}{c}{F}_{\mathrm{CR}}^x={\cos \mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^y={-\cos \mathrm{\γ}}_{\mathrm{CR}}\\ F_{\mathrm{CR}}^z={-\sin \mathrm{\γ}}_{\mathrm{CR}}\end{array}\right.$

The contribution coefficient vector of left front horizontal propeller (1) in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of right front horizontal propeller (4) in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of left back horizontal propeller (6) in three rotary freedom directions be calculated as follows:

The contribution coefficient vector of right back horizontal propeller (9) in three rotary freedom directions be calculated as follows:

Two vertical pusher 5 are expressed as at the contribution coefficient vector in three rotary freedom directions with be calculated as follows:

The control dispenser matrix L finally obtained by each angle of rake contribution coefficient vector is:

$L=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]$

Step 4, obtained m angle of rake thrust command vector T=[T of m × 1 by degree of freedom requirement command vector D and the result of the inner product LD controlling dispenser matrix L _fLt _fRt _aLt _aRt _cLt _cR] ^t, be calculated as follows formula, complete thrust thus and distribute:

$\left[\begin{array}{c}{T}_{\mathrm{FL}}\\ T_{\mathrm{FR}}\\ T_{\mathrm{AL}}\\ T_{\mathrm{AR}}\\ T_{\mathrm{CL}}\\ T_{\mathrm{CR}}\end{array}\right]=\left[\begin{array}{cccccc}{F}_{\mathrm{FL}}^x& F_{\mathrm{FL}}^y& F_{\mathrm{FL}}^z& M_{\mathrm{FL}}^x& M_{\mathrm{FL}}^y& M_{\mathrm{FL}}^z\\ F_{\mathrm{FR}}^x& F_{\mathrm{FR}}^y& F_{\mathrm{FR}}^z& M_{\mathrm{FR}}^x& M_{\mathrm{FR}}^y& M_{\mathrm{FR}}^z\\ F_{\mathrm{AL}}^x& F_{\mathrm{AL}}^y& F_{\mathrm{AL}}^z& M_{\mathrm{AL}}^x& M_{\mathrm{AL}}^y& M_{\mathrm{AL}}^z\\ F_{\mathrm{AR}}^x& F_{\mathrm{AR}}^y& F_{\mathrm{AR}}^z& M_{\mathrm{AR}}^x& M_{\mathrm{AR}}^y& M_{\mathrm{AR}}^z\\ F_{\mathrm{CL}}^x& F_{\mathrm{CL}}^y& F_{\mathrm{CL}}^z& M_{\mathrm{CL}}^x& M_{\mathrm{CL}}^y& M_{\mathrm{CL}}^z\\ F_{\mathrm{CR}}^x& F_{\mathrm{CR}}^y& F_{\mathrm{CR}}^z& M_{\mathrm{CR}}^x& M_{\mathrm{CR}}^y& M_{\mathrm{CR}}^z\end{array}\right]\left[\begin{array}{c}{\mathrm{\Γ}}_x\\ {\mathrm{\Γ}}_y\\ {\mathrm{\Γ}}_z\\ {\mathrm{\Ω}}_x\\ {\mathrm{\Ω}}_y\\ {\mathrm{\Ω}}_z\end{array}\right],$

Wherein, m is angle of rake number.