JP2007118702A - Position and posture control device and position and posture control method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To carry out position and posture control of a floating body capable of small sharp turns and to carry out the position and posture control of the floating body moving while maintaining the same posture not only in one direction but also in other various directions. <P>SOLUTION: This position and posture control device is furnished with six thrust generators 14 to respectively generate thrust, and any one of the thrust generators 14 is set to generate the thrust in different directions. Each of the thrust generators 14 is positioned so that a virtual straight line 30 extending in the thrust direction crosses with the virtual straight line 30 extending in the thrust direction of the neighboring thrust generator 14 on two virtual flat surfaces vertical with a virtual axis 40, an intersection point of the virtual straight line 30 on one of the virtual flat surfaces comes to be at each of top point positions of an equilateral triangle at a position of a center of gravity on the virtual axis 40 and an intersection point of the virtual straight line 30 on the other virtual flat surface comes to be at each of the top point positions of the equilateral triangle at the position of the center of gravity on the virtual axis 40. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

  The present invention relates to a position and orientation control apparatus and a position and orientation control method for controlling the position and orientation of a floating body.

An example of a floating body is a deep sea research boat. As an example of this deep sea research boat, there is “Shinkai 6500” published in Non-Patent Document 1 below, and this deep sea research boat has a main purpose of propelling the boat in the front-rear direction as shown in FIG. Are provided, a vertical thruster 92 responsible for vertical movement, and a horizontal thruster 93 responsible for turning the head in the left-right direction. The deep-sea research boat can control the thrust by the propulsion device 91 and the thrusters 92 and 93 to track the survey object in the survey sea area in a vertical and horizontal manner while capturing the survey object. . In this deep-sea research boat, the position and orientation are controlled by thrusts in the three axial directions of front and rear, up and down, and left and right, which is a nonholonomic constraint condition.
"Search June 27, 2005", Internet <URL;http://www.jamstec.go.jp/ships/shinkai3.html>

  The conventional deep-sea research boat is configured to give thrust in three axial directions, and is a nonholonomic constraint condition. For this reason, it is difficult to change the direction while staying at a certain point, and it is necessary to control the position and posture to desired values while gradually changing the floating point. As a result, there is a problem that posture control with a small turn cannot be performed. Further, there is a problem that it is difficult to move while maintaining the same posture in the left-right direction or the like, although it is possible to move in the front-rear direction while maintaining the same posture.

  Therefore, the present invention has been made in view of such points, and the object of the present invention is to enable control of the position and orientation of a floating body that is effective in turning, and not only in one direction but also in other various directions. Is to enable position and orientation control to move while maintaining the same posture.

  To achieve the above object, the present invention presupposes a position and orientation control device for controlling the position and orientation of a floating body, and controls each of the six thrust generators for generating thrust and the thrust generators. And each of the thrust generators is set to generate thrust in different directions.

  In the present invention, six thrust generators are provided in all different directions, thereby enabling position and orientation control in the six-axis direction that does not result in nonholonomic constraint conditions. Therefore, by adjusting the thrust generated by each thrust generator by the control unit, it is possible to control the position and orientation with a simple structure and to make a small turn, and to maintain the same posture not only in one direction but also in various directions. Position and orientation control can be performed while moving. Also, since each thrust generator is fixed in position, unlike the normal parallel mechanism in which the geometric shape changes as the link length expands and contracts, the position and orientation are controlled while re-estimating the shape parameters. There is no need, and once the geometric shape is set, position and orientation control using the shape parameter becomes possible. As a result, the control calculation can be made simpler, and quick and accurate control is possible.

  Here, when two virtual planes perpendicular to the virtual axis are defined, each thrust generator has a virtual straight line extending in the thrust direction and a virtual straight line extending in the thrust direction of an adjacent thrust generator on the virtual plane. Positioned to intersect, the intersection of the virtual lines on the one virtual plane becomes each vertex position of an equilateral triangle having a center of gravity position on the virtual axis, and the virtual line on the other virtual plane It is preferable that the intersections of the two are positioned so as to be the vertex positions of an equilateral triangle having a centroid position on the virtual axis.

  In this aspect, since each thrust generator is positioned so that the intersection of the virtual straight lines on the two virtual planes is the vertex position of the equilateral triangle, the position and orientation in a plane parallel to the virtual plane Control can be stably performed in various directions regardless of the moving direction and the rotating direction.

  And it is more preferable that each said thrust generator is positioned so that each intersection used as each vertex position of the said two regular triangles may comprise hexagons other than a regular hexagon seeing in the direction of the said virtual axis. .

  In this aspect, singular points in the position and orientation control are less likely to occur, so that the position and orientation of the floating body can be reliably converged to the target value.

  Further, it is preferable that each of the thrust generators is set so that the angle formed by the virtual straight line and the virtual plane is the same angle.

  In this aspect, since the same thing can be used for all the thrust generators, it is possible to suppress the calculation of the position and orientation control from becoming complicated.

（ただし、Ｒｃ＝−０．１、Ｈｃ＝０．０、ａ＝０．０４５、ｂ＝０．０７、ｃ＝１２、ｄ＝０．２５）によって描かれる楕円の内側の領域に前記Ｒ及びＨが属しているのが好ましい。 Further, the ratio of the radius of the virtual circle passing through each vertex of the regular triangle on the other virtual plane to the radius of the virtual circle passing through each vertex of the regular triangle on the one virtual plane is R, and the one virtual plane When the ratio of the distance between the two virtual planes to the radius of the upper virtual circle is H, the following formula (1)

(Where Rc = −0.1, Hc = 0.0, a = 0.045, b = 0.07, c = 12, d = 0.25) in the region inside the ellipse drawn by Preferably H belongs.

  In the thrust generator, an angle formed by the virtual straight line and the virtual plane may be set to 45 degrees.

  In this aspect, since manufacturing misalignment can be made difficult to occur, more accurate control can be performed.

  Further, the present invention adjusts the thrust generated by the six thrust generators fixed in directions that generate thrust in different directions, assuming a position and orientation control method for controlling the position and orientation of the floating body. To control the position and posture of the floating body.

  In this position and orientation control method, when two virtual planes perpendicular to the virtual axis are defined, each thrust generator has a virtual straight line extending in the thrust direction and a virtual straight line extending in the thrust direction of the adjacent thrust generator, Positioned so as to intersect on the virtual plane, and the intersection of the virtual straight lines on the one virtual plane is the vertex position of an equilateral triangle having a center of gravity on the virtual axis, and on the other virtual plane The position of the floating body is determined by adjusting the thrust generated by these thrust generators so that the intersections of the virtual straight lines in FIG. It is preferable to control.

  And it is preferable that each said thrust generator is positioned so that each intersection used as each vertex position of the said two regular triangles may comprise hexagons other than a regular hexagon seeing in the direction of the said virtual axis.

  Further, it is preferable that each of the thrust generators is set so that the angle formed by the virtual straight line and the virtual plane is the same angle.

  As described above, according to the present invention, the position and orientation control of the floating body that is effective in turning is possible, and the position and orientation control that moves while maintaining the same posture not only in one direction but also in various other directions. Can be made possible.

  Hereinafter, the best mode for carrying out the present invention will be described in detail with reference to the drawings.

  FIG. 1 shows a floating body to which a position and orientation control apparatus according to an embodiment of the present invention is applied. The floating body is configured as, for example, a submersible 10 that can float in water. The submersible craft 10 here means a thing that moves around in the water in order to perform searching, investigation, predetermined work, etc. underwater.

  The submersible craft 10 includes a main body 12 on which devices are mounted. Examples of devices include a control device, a camera, a work device, and a power source. Further, when the submersible craft 10 is a manned submersible craft, the equipment includes a cockpit or the like. In the present embodiment, the main body 12 is formed in a substantially spherical shape obtained by combining hemispherical split shapes.

  A thrust generator 14,..., 14 positioned through a frame body 16 is fixed to the main body 12. The thrust generators 14 are positioned with respect to the main body 12 by being attached to the frame body 16 fastened and fixed so as to circumscribe the circumference of the equator of the main body 12. The frame body 16 includes a base frame 18 that is a first frame and an end frame 20 that is a second frame. The base frame 18 and the end frame 20 are each formed in a hexagonal shape. The base frame 18 and the end frame 20 are each formed in a planar shape, and are arranged so that these two planes are parallel to each other.

  The frame body 16 is provided with connecting members 23,... 23, which are installed between the base frame 18 and the end frame 20. There are six connecting members 23,..., And each connecting member 23 is arranged in an inclined posture so as to form a V shape with the adjacent connecting member 23 with respect to the plane. The V-shaped vertices coincide with three vertex positions selected every other hexagonal vertices constituting the base frame 18 (or the end frame 20). That is, one end of each connecting member 23 is coupled at a hexagonal apex position in the base frame 18, and the other end is coupled at a hexagonal apex position in the end frame 20. And the connection part of the base frame 18 and the one end part of the three connection members 23, ..., 23 selected every other one is comprised so that it may become each vertex position of an equilateral triangle, and similarly the said connection The joints between the other ends of the members 23,..., 23 and the end frame 20 are configured so as to be at the apex positions of equilateral triangles.

  Six thrust generators 14 are provided (one is not shown), and each of these thrust generators 14 is fixed to the connecting member 23 via a support member 25. The thrust generators 14,..., 14 are arranged so as to surround the main body portion 12, and are inclined with respect to the plane so as to form a V shape with the adjacent thrust generators 14,. Arranged in posture.

  Each thrust generator 14 includes a rounded and elongated motor part 27 and a propeller part 28 provided at the tip of the motor part 27, and the propeller part 28 is a motor built in the motor part 27. It is the structure which generates a thrust by driving by. Each thrust generator 14 is attached such that the axis of the thrust generated is in a plane including the center of the main body 12 and the connecting member 23.

  Here, the positional relationship between the thrust generators 14,. That is, as shown in FIG. 2, when virtual lines 30,... 30 extending in the direction of thrust generated by the thrust generators 14,. It intersects with a virtual straight line 30 extending in the direction of thrust generated by the thrust generator 14. Moreover, this intersection is located on a virtual plane (first virtual plane) 32 including the base frame 18 or a virtual plane (second virtual plane) 34 including the end frame 20 as shown in FIG. . More specifically, the imaginary straight line 30 of a certain thrust generator 14 intersects the imaginary straight line 30 of the thrust generator 14 adjacent to one side on the first imaginary plane 32, but is adjacent to the other side (opposite side). The thrust generator 14 intersects the virtual line 30 on the second virtual plane 34. As a result, there are three intersections on the first virtual plane 32 and there are also three intersections on the second virtual plane 34. As shown in FIG. 3A, the three intersections on the first virtual plane 32 are the vertices of an equilateral triangle 36 having a center of gravity on the virtual axis 40, and on the second virtual plane 34. The three intersections are the vertices of an equilateral triangle 38 having a centroid position on the virtual axis 40. A virtual circle passing through each vertex of the regular triangle 36 on the first virtual plane 32 is referred to as a first virtual circle 41, and a virtual circle passing through each vertex of the regular triangle 38 on the second virtual plane 34 is referred to as a second virtual circle 42. I will call it. Both virtual planes 32 and 34 are planes perpendicular to the virtual axis 40 and are parallel to each other.

  As shown in FIG. 4, the submersible craft 10 is provided with a control unit 44 that controls the thrust generated by the thrust generators 14,. When the thrust generated by each thrust generator 14,..., 14 is represented by a vector, the control unit 44 controls the movement and posture of the submersible craft 10 by the sum of the vectors. 46 and a motor control unit 47 are included. The thrust deriving unit 46 derives a thrust to be generated by each of the thrust generators 14. The motor control unit 47 calculates the duty ratio of the motor so as to cause the thrust generators 14,... 14 to generate the thrust obtained by the thrust deriving unit 46, and controls the rotational direction and the rotational speed of the motor. For example, the control unit 44 may be provided in the main body unit 12, or when the submersible craft 10 is well-developed and connected to a mother ship (not shown), the control unit 44 is connected to the mother ship. You may make it provide.

と表すことができる。 Here, the derivation of the thrust generated by each thrust generator 14,... 14 by the thrust derivation unit 46 will be described. The resultant force FG acting on the submersible 10 is determined as a concentrated load acting on the center O of the first virtual circle 41. As shown in FIG. 5, at the intersections A, B, and C of the three virtual straight lines 30 on the first virtual plane 32, thrust Fi (i = 1) by the thrust generators 14,. A natural number of ~ 6). X-direction component of the resultant force _{F G,} respectively y direction component and z-direction components Fx, when the Fy and Fz,
F _G = Fx + Fy + Fz (2)
The following relational expression is established, and these Fx, Fy, Fz are used as unit vectors u _ix , u _iy , u _iz in the x direction, y direction, and z direction,

It can be expressed as.

なる関係が成立する。 Next, the rotational moment _NG around the center O acting on the submersible 10 can be obtained by the outer product of each thrust Fi and the position vector of the intersections A, B, C with respect to the center O.

This relationship is established.

により推力が求められる。ここで、Ｃ_ｌは、推力発生器１４，・・，１４の形状及び配置関係に依存する行列である。 As described above, it is possible to adjust the resultant force applied to the center O by adjusting each thrust, and when actually adjusting, the following formula (6)

Therefore, thrust is required. Here, C ₁ is a matrix that depends on the shape and arrangement relationship of the thrust generators 14.

Next, a preferred range for the arrangement relationship of the thrust generators 14,. This preferable range means a range in which the position and orientation control in various directions is not easily affected even when a disturbance is applied to the thrust. In order to obtain this preferable range, a model based on the Stewart platform shown in FIG. 6 was adopted. The following conditions were adopted as the constraint conditions.
1. The upper and lower disks 54 and 56 are parallel to each other.
2. An axis 58 connecting the centers of the respective disks is perpendicular to the disks 54 and 56.
3. The disks 54 and 56 and the two links 60 and 60 are connected by one node.
4). The nodes of the disks 54 and 56 and the links 60,..., 60 are evenly arranged on the circumferences of the disks 54 and 56. That is, the interval is 120 degrees.
5. Each link 60,..., 60 has the same length.

で表現される評価値Ｓを求めた。この評価値Ｓは、目標並進力の大きさ｜Ｆ｜や目標回転モーメント｜Ｎ｜を正規化したものである。（Ｆ Ｎ）^Ｔの様々な組み合わせにより評価値Ｓの等高線図を作成した。この等高線図を図８及び図９に示す。この図９の横軸は、下部円盤５６の半径を１としたときの上部円盤５４の半径Ｒの対数値であり、縦軸は、下部円盤５６の半径を１としたときの両円盤間の間隔Ｈの対数値である。図８は評価値Ｓを三次元的に示したものである。 Link 60, ..., and that thrust is added in the direction of 60, center (F ^N) each necessary link to generate a ^T 60 of the upper disk 54, ..., 60 the direction of the thrust force _{F l} Ask. Then, it is assumed that an error of 10% at maximum is given to the obtained thrust F ₁ , and as a result, (Fc Nc) is generated at the center of the upper disk 54 (see FIG. 7). Here, each direction, size, and additional error of (F N) ^T were randomly generated so that | F | ≠ 0 and | N | ≠ 0. Then, as an evaluation function, formula (7)

The evaluation value S expressed by This evaluation value S is obtained by normalizing the target translational force magnitude | F | and the target rotational moment | N |. (F N) Contour maps of the evaluation value S were created by various combinations of ^T. This contour map is shown in FIGS. The horizontal axis of FIG. 9 is a logarithmic value of the radius R of the upper disk 54 when the radius of the lower disk 56 is 1, and the vertical axis is between the two disks when the radius of the lower disk 56 is 1. It is a logarithmic value of the interval H. FIG. 8 shows the evaluation value S three-dimensionally.

  In order to obtain the optimum value of the evaluation value S, a search was alternately performed in the R direction and the H direction using the steepest descent method. At this time, a quadratic approximate expression of the evaluation value S was obtained for the logarithmic values of R and H, and a search method for sequentially obtaining the minimum value in each direction by the approximate expression was also used. As a result, R = 0.80, H = 0.97, and an angle of 43 degrees between the links 60,.

で近似することができる。楕円の中心座標（ｌｏｇ(Ｒｃ)、ｌｏｇ(Ｈｃ)）は、最適値Ｒ＝０．８０、Ｈ＝０．９７の対数値の近似値としての（−０．１，０．０）である。 Moreover, the approximation of the contour line of the evaluation value S is shown in FIG. 10 which is a logarithmic graph with logarithmic values of R and H as axes. The number in the figure is the evaluation value S, and the evaluation value S expressed as a contour line is the following formula (1) representing an ellipse:

Can be approximated by The center coordinates (log (Rc), log (Hc)) of the ellipse are (−0.1, 0.0) as approximate values of logarithmic values of optimum values R = 0.80 and H = 0.97. .

  As a preferable range, the coefficient in the expression (7) in which the evaluation value S is less than 1.0 is an ellipse represented by a = 0.045, b = 0.07, c = 12, and d = 0.25. Of the ellipse represented by a = 0.05, b = 0.07, c = 5.3, and d = 0.25, where the evaluation value S is less than 0.5. More preferably, the inner side of the ellipse in which the coefficient with the evaluation value S less than 0.3 is represented by a = 0.07, b = 0.09, c = 1.3, d = 0.30 is more preferable, The inside of the ellipse represented by a = 0.07, b = 0.09, c = 0.11, and d = 0.35, where the evaluation value S is less than 0.23, is more preferable.

  Retrospectively, in the present embodiment, the regular triangle 36 on the first virtual plane 32 is set to be slightly larger than the regular triangle 38 on the second virtual plane 34, and the radius of the first virtual circle 41 is set to 1. Then, the radius of the second virtual circle 42 (corresponding to the radius R of the upper disk 54) is 0.80. When the radius of the first virtual circle 41 is 1, the distance between the virtual planes 32 and 34 (corresponding to the distance H between both disks) is 0.97. And the inclination angle which the said virtual straight line 30, ..., 30 makes with each virtual plane 32,34 is set to the same angle, and the angle is 43 degree | times. Therefore, even when a disturbance is added to the thrust, it is possible to achieve an arrangement relationship that is least susceptible to the influence of any direction. The tilt angle may be 45 degrees. In this case, the radius of the second virtual circle 42 is 0.81, and the interval between both virtual planes is 0.92. When the tilt angle is set to 45 degrees, the frame body 16 can be easily manufactured. Therefore, it is possible to make the frame body 16 less susceptible to misalignment while being less susceptible to thrust error. Even when the tilt angle is 45 degrees, the evaluation value S is less than 0.23, so that the position and orientation control in each direction can be performed accurately without being affected by disturbance.

  With respect to the submersible craft 10 configured as described above, an experiment was conducted in which a prototype small craft was moved in a water tank. Each of translation and rotation will be described for the verification result. The aquarium is 500mm in height, 1000mm in width, and 450mm in depth. For translational motion, the translational thrust in the X-axis direction is kept constant for about 7 seconds, and the movement trajectory at that time is measured by a CCD camera from the side of the aquarium. Continuous shooting. The result is shown in FIG. Although the propulsive force is constant, the speed converges early due to viscous resistance, and it can be seen that the prototype small boat is moving at a substantially constant speed in the X-axis direction. Further, the vertical movement (movement in the Y-axis direction) is suppressed within about ± 5 mm, and it can be seen that the translational movement is performed while maintaining the posture.

  Next, FIG. 12 shows an example of the result when the rotational movement around the Z axis is performed. This rotational motion is the result of rotating for about 2 seconds. Like the translational motion, the rotational angle increases linearly because of the balance between the viscous resistance force and the resultant force of thrust. You can see that it is rotating. Moreover, it has been confirmed that vertical movement and translational movement do not occur by observation from the side and from above. Therefore, free movement and posture control in the six-axis directions in water are possible.

  As described above, according to the present embodiment, by providing six thrust generators 14,..., 14 in all different directions, it is possible to perform 6-axis position and orientation control that is not a nonholonomic constraint condition. Become. Therefore, by adjusting the thrust generated by each of the thrust generators 14,..., The position and orientation control can be performed with a simple structure and with a small turn, and not only in one direction but also in various directions. Thus, it is possible to perform position and orientation control while moving while maintaining the same posture. Moreover, since each thrust generator 14,..., 14 is fixed, it is not necessary to perform position and orientation control while re-estimating the shape parameter, unlike the case where the geometric shape changes like a parallel mechanism. Once a geometric shape is set, position and orientation control using that shape parameter is always possible. As a result, the control calculation can be made simpler, and quick and accurate control is possible.

  Further, in the present embodiment, the intersections of the virtual straight lines 30,... 30 are the vertex positions of the regular triangle 36 in the first virtual plane 32 or the vertex positions of the regular triangle 38 in the second virtual plane 34. Since the thrust generators 14,... Are positioned at the same position, the position and orientation control in a plane parallel to the virtual planes 32, 34 can be stably performed in various directions regardless of the moving direction and the rotating direction. It can be carried out. In addition, in the present embodiment, the thrust generators 14,..., 14 are positioned so that each intersection point forms a hexagon other than a regular hexagon when viewed in the direction of the virtual axis 40. As a result, the position and posture of the submersible craft 10 can be reliably converged to the target value.

  In the present embodiment, since each of the thrust generators 14,..., 14 is set to have the same angle with the virtual plane, all the thrust generators 14,. The same thing can be used, and as a result, it can suppress that the calculation of position and orientation control becomes complicated.

  In addition, this invention is not restricted to the said embodiment, In the range of the invention described in the claim, it can change suitably. For example, in the above-described embodiment, the thrust generators 14 are fixed so that the propeller portion 28 is positioned in the vicinity of the base frame 18, but the propeller portion 28 is intermediate between the end frame 20 and the base frame 18. It may be arranged at a position.

  Moreover, in the said embodiment, although the frame body 16 was fixed to the main-body part 12, and it was set as the structure which attaches the thrust generators 14, ..., 14 to the frame body 16, it is not restricted to this. For example, as shown in FIG. 13, it is good also as a structure which provides the thrust generators 14, ..., 14 in the main-body part 12 of the submersible craft 10 directly. That is, the main body portion 12 is formed in, for example, a spherical shape, and six through holes 70,... 70 are formed in the main body portion 12. Each of the through holes 70 is provided with a thrust generator 14, and when the thrust generator 14 is driven, water is discharged through the through holes 70, and thrust is directed in the direction of the through holes 70. appear. The direction of the through holes 70,... 70 is configured in the same manner as the direction of the connecting members 23,. In this aspect, not only the frame body 16 can be omitted, but also the flow resistance can be reduced, and more accurate control is possible.

  Further, instead of the thrust generators 14,..., 14 shown in FIG. 1, as shown in FIG. 14, if the propeller 28 has a larger diameter than the motor 27, the propeller 28 In either case, the thrust can be effectively generated.

  In the above embodiment, an example in which the submersible craft 10 is configured as an example of a floating body has been described. However, the embodiment is not limited thereto, and the floating body may be configured as a balloon, an airship, a submarine, an underwater buoy, a spacecraft, or the like. May be.

1 is a perspective view of a submersible craft according to an embodiment of the present invention. It is a conceptual diagram for demonstrating the geometric arrangement | positioning relationship of a virtual straight line. (A) It is explanatory drawing for demonstrating the positional relationship of the virtual straight line seen from the virtual axis direction, the 1st virtual circle, the 2nd virtual circle, and two equilateral triangles, (b) It sees in the direction orthogonal to a virtual axis. It is explanatory drawing for demonstrating the 1st virtual plane and 2nd virtual plane. It is a block diagram which shows the function contained in a control part. It is a figure which shows the vector used in order to derive | lead-out the thrust which each thrust generator produces | generates. It is a figure which shows the model based on a Stewart platform. It is a figure which shows the model based on a Stewart platform. 3 is a contour map of an evaluation value S. FIG. 3 is a contour map of an evaluation value S. FIG. 3 is a contour map of an evaluation value S. FIG. It is a characteristic view which shows an example of a result when performing the control which moves the said submersible craft in the X-axis direction. It is a characteristic view which shows an example of a result when performing the control which rotates the said submersible wing around a Z-axis. It is a perspective view which shows roughly the submersible craft which concerns on other embodiment of this invention. It is a figure which shows the thrust generator provided in the submersible craft which concerns on other embodiment of this invention. It is a side view of the conventional deep sea research boat.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 Submersible 14 Thrust generator 30 Virtual straight line 32 1st virtual plane 34 2nd virtual plane 36 Eight triangle 38 Eight triangle 40 Virtual axis 41 1st virtual circle 42 2nd virtual circle

Claims (10)

A position and orientation control device for controlling the position and orientation of a floating body,
6 thrust generators each generating thrust,
A control unit for controlling each of the thrust generators,
A position and orientation control apparatus in which each of the thrust generators is set to generate thrust in different directions. When two virtual planes perpendicular to the virtual axis are defined,
Each thrust generator is positioned so that a virtual straight line extending in the thrust direction intersects a virtual straight line extending in the thrust direction of an adjacent thrust generator on the virtual plane, and the virtual line on the one virtual plane is set. The intersection of the straight lines becomes each vertex position of the equilateral triangle having the center of gravity on the virtual axis, and the intersection of the virtual straight line on the other virtual plane is each vertex position of the equilateral triangle having the center of gravity on the virtual axis The position and orientation control apparatus according to claim 1, wherein the position and orientation control apparatus is positioned so that   3. The thrust generators according to claim 2, wherein each of the thrust generators is positioned such that each intersection point that is a vertex position of the two regular triangles forms a hexagon other than a regular hexagon when viewed in the direction of the virtual axis. Position and orientation control device.   4. The position and orientation control apparatus according to claim 2, wherein each of the thrust generators is set so that an angle formed by the virtual straight line and the virtual plane is the same angle. 5. The ratio of the radius of the virtual circle passing through each vertex of the equilateral triangle on the other virtual plane to the radius of the virtual circle passing through each vertex of the equilateral triangle on the one virtual plane is R, and on the one virtual plane When the ratio of the distance between the two virtual planes to the radius of the virtual circle is H, the following formula (1)

(Where Rc = −0.1, Hc = 0.0, a = 0.045, b = 0.07, c = 12, d = 0.25) in the region inside the ellipse drawn by The position and orientation control apparatus according to claim 2 or 3, wherein H belongs.   The position and orientation control apparatus according to claim 5, wherein the thrust generator is configured such that an angle formed between the virtual straight line and the virtual plane is set to 45 degrees. A position and orientation control method for controlling the position and orientation of a floating body,
A position and orientation control method for controlling the position and orientation of the floating body by adjusting thrusts generated by six thrust generators fixed in directions in which thrust is generated in different directions. When two virtual planes perpendicular to the virtual axis are defined,
Each thrust generator is positioned such that a virtual straight line extending in the thrust direction intersects a virtual straight line extending in the thrust direction of an adjacent thrust generator on the virtual plane, and the virtual line on the one virtual plane is The intersection of the straight lines becomes each vertex position of the equilateral triangle having the center of gravity on the virtual axis, and the intersection of the virtual straight line on the other virtual plane is each vertex position of the equilateral triangle having the center of gravity on the virtual axis The position and orientation control method according to claim 7, wherein the position and orientation of the floating body are controlled by adjusting the thrust generated by the thrust generators.   9. The thrust generators according to claim 8, wherein each of the thrust generators is positioned such that each intersection point that is a vertex position of each of the two regular triangles forms a hexagon other than a regular hexagon when viewed in the direction of the virtual axis. Position and orientation control method.   10. The position and orientation control method according to claim 8, wherein each of the thrust generators is set such that an angle formed between the virtual straight line and the virtual plane is the same.

Images